From 3238841dc644572cab5e0e6ae5195d197fcdf01e Mon Sep 17 00:00:00 2001 From: blobt <380255922@qq.com> Date: Wed, 29 Jan 2020 10:17:24 +0800 Subject: [PATCH] add example --- .../lesson001-windows/lesson001-window.vcproj | 197 + example/lesson001-windows/window.cpp | 76 + .../lesson002-windows.vcproj | 197 + example/lesson002-windows/window2.cpp | 117 + example/lesson003-point/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson003-point/Raster.cpp | 17 + example/lesson003-point/Raster.h | 60 + .../lesson003-point/lesson003-point.vcproj | 209 + example/lesson003-point/lesson003.cpp | 118 + example/lesson004-point改进/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson004-point改进/Raster.cpp | 54 + example/lesson004-point改进/Raster.h | 31 + .../lesson004-point改进.vcproj | 209 + example/lesson004-point改进/lesson004.cpp | 118 + example/lesson005-line/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson005-line/Raster.cpp | 86 + example/lesson005-line/Raster.h | 33 + example/lesson005-line/lesson005-line.vcproj | 209 + example/lesson005-line/lesson005.cpp | 117 + example/lesson006-line2/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson006-line2/Raster.cpp | 53 + example/lesson006-line2/Raster.h | 134 + .../lesson006-line2/lesson006-line2.vcproj | 209 + example/lesson006-line2/lesson006.cpp | 117 + example/lesson007-line-color/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson007-line-color/Raster.cpp | 118 + example/lesson007-line-color/Raster.h | 33 + .../lesson007-line-color.vcproj | 209 + example/lesson007-line-color/lesson007.cpp | 117 + example/lesson008-lineStrip/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson008-lineStrip/Raster.cpp | 116 + example/lesson008-lineStrip/Raster.h | 94 + .../lesson008-lineStrip.vcproj | 209 + example/lesson008-lineStrip/lesson008.cpp | 137 + example/lesson009-贝塞尔/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson009-贝塞尔/Raster.cpp | 116 + example/lesson009-贝塞尔/Raster.h | 94 + .../lesson009-贝塞尔.vcproj | 209 + example/lesson009-贝塞尔/lesson009.cpp | 145 + example/lesson010-rect/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson010-rect/Raster.cpp | 161 + example/lesson010-rect/Raster.h | 67 + example/lesson010-rect/lesson010-rect.vcproj | 209 + example/lesson010-rect/lesson010.cpp | 147 + example/lesson011-rectEx/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson011-rectEx/Raster.cpp | 161 + example/lesson011-rectEx/Raster.h | 73 + .../lesson011-rectEx/lesson011-rectEx.vcproj | 209 + example/lesson011-rectEx/lesson011.cpp | 147 + example/lesson012-rect-color/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson012-rect-color/Raster.cpp | 178 + example/lesson012-rect-color/Raster.h | 82 + .../lesson012-rect-color.vcproj | 209 + example/lesson012-rect-color/lesson012.cpp | 164 + example/lesson013-triangle/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson013-triangle/Raster.cpp | 202 + example/lesson013-triangle/Raster.h | 69 + .../lesson013-triangle.vcproj | 209 + example/lesson013-triangle/lesson013.cpp | 164 + example/lesson014-span/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson014-span/Raster.cpp | 202 + example/lesson014-span/Raster.h | 149 + example/lesson014-span/lesson014-span.vcproj | 209 + example/lesson014-span/lesson014.cpp | 164 + example/lesson015-span/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson015-span/Raster.cpp | 202 + example/lesson015-span/Raster.h | 183 + example/lesson015-span/lesson015-span.vcproj | 209 + example/lesson015-span/lesson015.cpp | 122 + example/lesson016-triangle-color/CELLMath.hpp | 5930 ++++++++++++++++ example/lesson016-triangle-color/Raster.cpp | 202 + example/lesson016-triangle-color/Raster.h | 204 + .../lesson016-triangle-color.vcproj | 209 + .../lesson016-triangle-color/lesson016.cpp | 126 + example/lesson017-优化/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson017-优化/CELLTimestamp.hpp | 53 + example/lesson017-优化/Raster.cpp | 202 + example/lesson017-优化/Raster.h | 208 + .../lesson017-优化/lesson017-优化.vcproj | 211 + example/lesson017-优化/lesson017.cpp | 142 + example/lesson018-优化2/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson018-优化2/CELLTimestamp.hpp | 53 + example/lesson018-优化2/Raster.cpp | 202 + example/lesson018-优化2/Raster.h | 222 + .../lesson018-优化2.vcproj | 211 + example/lesson018-优化2/lesson018.cpp | 142 + example/lesson019-优化3/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson019-优化3/CELLTimestamp.hpp | 53 + example/lesson019-优化3/Raster.cpp | 202 + example/lesson019-优化3/Raster.h | 229 + .../lesson019-优化3.vcproj | 211 + example/lesson019-优化3/lesson019.cpp | 142 + example/lesson020-优化4/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson020-优化4/CELLTimestamp.hpp | 53 + example/lesson020-优化4/Raster.cpp | 202 + example/lesson020-优化4/Raster.h | 232 + example/lesson020-优化4/lesson019.cpp | 142 + .../lesson020-优化4.vcproj | 211 + example/lesson200-drawImage/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson200-drawImage/CELLTimestamp.hpp | 53 + example/lesson200-drawImage/Raster.cpp | 202 + example/lesson200-drawImage/Raster.h | 226 + .../lesson200-drawImage.vcproj | 210 + example/lesson200-drawImage/lesson200.cpp | 144 + example/lesson201-freeImage/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson201-freeImage/CELLTimestamp.hpp | 53 + example/lesson201-freeImage/Raster.cpp | 230 + example/lesson201-freeImage/Raster.h | 226 + .../lesson201-freeImage.vcproj | 214 + example/lesson201-freeImage/lesson201.cpp | 144 + example/lesson202-ImageClass/CELLMath.hpp | 5942 ++++++++++++++++ .../lesson202-ImageClass/CELLTimestamp.hpp | 53 + example/lesson202-ImageClass/Image.hpp | 51 + example/lesson202-ImageClass/Raster.cpp | 231 + example/lesson202-ImageClass/Raster.h | 228 + .../lesson202-ImageClass.vcproj | 219 + example/lesson202-ImageClass/lesson202.cpp | 163 + example/lesson203-color-key/CELLMath.hpp | 5942 ++++++++++++++++ example/lesson203-color-key/CELLTimestamp.hpp | 53 + example/lesson203-color-key/Image.hpp | 51 + example/lesson203-color-key/Raster.cpp | 283 + example/lesson203-color-key/Raster.h | 214 + .../lesson203-color-key.vcproj | 219 + example/lesson203-color-key/lesson203.cpp | 166 + example/lesson204-alpha-test/CELLMath.hpp | 5942 ++++++++++++++++ .../lesson204-alpha-test/CELLTimestamp.hpp | 53 + example/lesson204-alpha-test/Image.hpp | 51 + example/lesson204-alpha-test/Raster.cpp | 283 + example/lesson204-alpha-test/Raster.h | 235 + .../lesson204-alpha-test.vcproj | 219 + example/lesson204-alpha-test/lesson204.cpp | 166 + example/lesson205-alpha-blend/CELLMath.hpp | 5942 ++++++++++++++++ .../lesson205-alpha-blend/CELLTimestamp.hpp | 53 + example/lesson205-alpha-blend/Image.hpp | 51 + example/lesson205-alpha-blend/Raster.cpp | 324 + example/lesson205-alpha-blend/Raster.h | 222 + .../lesson205-alpha-blend.vcproj | 219 + example/lesson205-alpha-blend/lesson205.cpp | 167 + example/lesson206-alpha透明/CELLMath.hpp | 5942 ++++++++++++++++ .../lesson206-alpha透明/CELLTimestamp.hpp | 53 + example/lesson206-alpha透明/Image.hpp | 51 + example/lesson206-alpha透明/Raster.cpp | 323 + example/lesson206-alpha透明/Raster.h | 244 + example/lesson206-alpha透明/lesson205.cpp | 167 + .../lesson206-alpha透明.vcproj | 219 + example/lesson206-alpha透明/lesson206.cpp | 169 + example/lesson207-drawPart/CELLMath.hpp | 5942 ++++++++++++++++ example/lesson207-drawPart/CELLTimestamp.hpp | 53 + example/lesson207-drawPart/Image.hpp | 51 + example/lesson207-drawPart/Raster.cpp | 344 + example/lesson207-drawPart/Raster.h | 244 + example/lesson207-drawPart/lesson205.cpp | 167 + .../lesson207-drawPart.vcproj | 219 + example/lesson207-drawPart/lesson207.cpp | 178 + example/lesson208-scale/CELLMath.hpp | 5942 ++++++++++++++++ example/lesson208-scale/CELLTimestamp.hpp | 53 + example/lesson208-scale/Image.hpp | 51 + example/lesson208-scale/Raster.cpp | 362 + example/lesson208-scale/Raster.h | 246 + example/lesson208-scale/lesson205.cpp | 167 + .../lesson208-scale/lesson208-scale.vcproj | 219 + example/lesson208-scale/lesson208.cpp | 181 + .../lesson209-scale-高质量/CELLMath.hpp | 5942 ++++++++++++++++ .../CELLTimestamp.hpp | 53 + example/lesson209-scale-高质量/Image.hpp | 51 + example/lesson209-scale-高质量/Raster.cpp | 362 + example/lesson209-scale-高质量/Raster.h | 246 + .../lesson209-scale-高质量.vcproj | 219 + .../lesson209-scale-高质量/lesson209.cpp | 181 + example/lesson210-总结/CELLMath.hpp | 5942 ++++++++++++++++ example/lesson210-总结/CELLTimestamp.hpp | 53 + example/lesson210-总结/Image.hpp | 51 + example/lesson210-总结/Raster.cpp | 362 + example/lesson210-总结/Raster.h | 246 + .../lesson210-总结/lesson210-总结.vcproj | 219 + example/lesson210-总结/lesson210.cpp | 181 + example/lesson300-纹理/CELLMath.hpp | 5950 +++++++++++++++++ example/lesson300-纹理/CELLTimestamp.hpp | 53 + example/lesson300-纹理/Image.hpp | 51 + example/lesson300-纹理/Raster.cpp | 63 + example/lesson300-纹理/Raster.h | 257 + .../lesson300-纹理/lesson300-纹理.vcproj | 219 + example/lesson300-纹理/lesson300.cpp | 177 + example/lesson301-纹理/CELLMath.hpp | 5950 +++++++++++++++++ example/lesson301-纹理/CELLTimestamp.hpp | 53 + example/lesson301-纹理/Image.hpp | 60 + example/lesson301-纹理/Raster.cpp | 63 + example/lesson301-纹理/Raster.h | 263 + .../lesson301-纹理/lesson301-纹理.vcproj | 219 + example/lesson301-纹理/lesson301.cpp | 185 + .../lesson302-纹理-color-blend/CELLMath.hpp | 5938 ++++++++++++++++ .../CELLTimestamp.hpp | 53 + .../lesson302-纹理-color-blend/Image.hpp | 60 + .../lesson302-纹理-color-blend/Raster.cpp | 63 + example/lesson302-纹理-color-blend/Raster.h | 260 + .../lesson302-纹理-color-blend.vcproj | 219 + .../lesson302.cpp | 185 + example/lesson303-状态机/CELLMath.hpp | 5938 ++++++++++++++++ example/lesson303-状态机/CELLTimestamp.hpp | 53 + example/lesson303-状态机/Image.hpp | 60 + example/lesson303-状态机/Raster.cpp | 66 + example/lesson303-状态机/Raster.h | 311 + .../lesson303-状态机.vcproj | 219 + example/lesson303-状态机/lesson303.cpp | 204 + example/lesson304-状态机2/CELLMath.hpp | 5938 ++++++++++++++++ .../lesson304-状态机2/CELLTimestamp.hpp | 53 + example/lesson304-状态机2/Image.hpp | 60 + example/lesson304-状态机2/Raster.cpp | 66 + example/lesson304-状态机2/Raster.h | 376 ++ .../lesson304-状态机2.vcproj | 219 + example/lesson304-状态机2/lesson304.cpp | 204 + example/lesson305-状态机3/CELLMath.hpp | 5938 ++++++++++++++++ .../lesson305-状态机3/CELLTimestamp.hpp | 53 + example/lesson305-状态机3/Image.hpp | 60 + example/lesson305-状态机3/Raster.cpp | 77 + example/lesson305-状态机3/Raster.h | 418 ++ .../lesson305-状态机3.vcproj | 219 + example/lesson305-状态机3/lesson305.cpp | 210 + .../CELLMath.hpp | 5938 ++++++++++++++++ .../CELLTimestamp.hpp | 53 + .../lesson306-纹理包装-重复/Image.hpp | 60 + .../lesson306-纹理包装-重复/Raster.cpp | 77 + .../lesson306-纹理包装-重复/Raster.h | 418 ++ .../lesson306-纹理包装-重复.vcproj | 219 + .../lesson306.cpp | 206 + .../CELLMath.hpp | 5938 ++++++++++++++++ .../CELLTimestamp.hpp | 53 + .../lesson307-纹理包装-clamage/Image.hpp | 80 + .../lesson307-纹理包装-clamage/Raster.cpp | 77 + .../lesson307-纹理包装-clamage/Raster.h | 418 ++ .../lesson307-纹理包装-clamage.vcproj | 219 + .../lesson307.cpp | 201 + example/lesson308-二维操作矩阵/1.ppt | 0 233 files changed, 250740 insertions(+) create mode 100644 example/lesson001-windows/lesson001-window.vcproj create mode 100644 example/lesson001-windows/window.cpp create mode 100644 example/lesson002-windows/lesson002-windows.vcproj create mode 100644 example/lesson002-windows/window2.cpp create mode 100644 example/lesson003-point/CELLMath.hpp create mode 100644 example/lesson003-point/Raster.cpp create mode 100644 example/lesson003-point/Raster.h create mode 100644 example/lesson003-point/lesson003-point.vcproj create mode 100644 example/lesson003-point/lesson003.cpp create mode 100644 example/lesson004-point改进/CELLMath.hpp create mode 100644 example/lesson004-point改进/Raster.cpp create mode 100644 example/lesson004-point改进/Raster.h create mode 100644 example/lesson004-point改进/lesson004-point改进.vcproj create mode 100644 example/lesson004-point改进/lesson004.cpp create mode 100644 example/lesson005-line/CELLMath.hpp create mode 100644 example/lesson005-line/Raster.cpp create mode 100644 example/lesson005-line/Raster.h create mode 100644 example/lesson005-line/lesson005-line.vcproj create mode 100644 example/lesson005-line/lesson005.cpp create mode 100644 example/lesson006-line2/CELLMath.hpp create mode 100644 example/lesson006-line2/Raster.cpp create mode 100644 example/lesson006-line2/Raster.h create mode 100644 example/lesson006-line2/lesson006-line2.vcproj create mode 100644 example/lesson006-line2/lesson006.cpp create mode 100644 example/lesson007-line-color/CELLMath.hpp create mode 100644 example/lesson007-line-color/Raster.cpp create mode 100644 example/lesson007-line-color/Raster.h create mode 100644 example/lesson007-line-color/lesson007-line-color.vcproj create mode 100644 example/lesson007-line-color/lesson007.cpp create mode 100644 example/lesson008-lineStrip/CELLMath.hpp create mode 100644 example/lesson008-lineStrip/Raster.cpp create mode 100644 example/lesson008-lineStrip/Raster.h create mode 100644 example/lesson008-lineStrip/lesson008-lineStrip.vcproj create mode 100644 example/lesson008-lineStrip/lesson008.cpp create mode 100644 example/lesson009-贝塞尔/CELLMath.hpp create mode 100644 example/lesson009-贝塞尔/Raster.cpp create mode 100644 example/lesson009-贝塞尔/Raster.h create mode 100644 example/lesson009-贝塞尔/lesson009-贝塞尔.vcproj create mode 100644 example/lesson009-贝塞尔/lesson009.cpp create mode 100644 example/lesson010-rect/CELLMath.hpp create mode 100644 example/lesson010-rect/Raster.cpp create mode 100644 example/lesson010-rect/Raster.h create mode 100644 example/lesson010-rect/lesson010-rect.vcproj create mode 100644 example/lesson010-rect/lesson010.cpp create mode 100644 example/lesson011-rectEx/CELLMath.hpp create mode 100644 example/lesson011-rectEx/Raster.cpp create mode 100644 example/lesson011-rectEx/Raster.h create mode 100644 example/lesson011-rectEx/lesson011-rectEx.vcproj create mode 100644 example/lesson011-rectEx/lesson011.cpp create mode 100644 example/lesson012-rect-color/CELLMath.hpp create mode 100644 example/lesson012-rect-color/Raster.cpp create mode 100644 example/lesson012-rect-color/Raster.h create mode 100644 example/lesson012-rect-color/lesson012-rect-color.vcproj create mode 100644 example/lesson012-rect-color/lesson012.cpp create mode 100644 example/lesson013-triangle/CELLMath.hpp create mode 100644 example/lesson013-triangle/Raster.cpp create mode 100644 example/lesson013-triangle/Raster.h create mode 100644 example/lesson013-triangle/lesson013-triangle.vcproj create mode 100644 example/lesson013-triangle/lesson013.cpp create mode 100644 example/lesson014-span/CELLMath.hpp create mode 100644 example/lesson014-span/Raster.cpp create mode 100644 example/lesson014-span/Raster.h create mode 100644 example/lesson014-span/lesson014-span.vcproj create mode 100644 example/lesson014-span/lesson014.cpp create mode 100644 example/lesson015-span/CELLMath.hpp create mode 100644 example/lesson015-span/Raster.cpp create mode 100644 example/lesson015-span/Raster.h create mode 100644 example/lesson015-span/lesson015-span.vcproj create mode 100644 example/lesson015-span/lesson015.cpp create mode 100644 example/lesson016-triangle-color/CELLMath.hpp create mode 100644 example/lesson016-triangle-color/Raster.cpp create mode 100644 example/lesson016-triangle-color/Raster.h create mode 100644 example/lesson016-triangle-color/lesson016-triangle-color.vcproj create mode 100644 example/lesson016-triangle-color/lesson016.cpp create mode 100644 example/lesson017-优化/CELLMath.hpp create mode 100644 example/lesson017-优化/CELLTimestamp.hpp create mode 100644 example/lesson017-优化/Raster.cpp create mode 100644 example/lesson017-优化/Raster.h create mode 100644 example/lesson017-优化/lesson017-优化.vcproj create mode 100644 example/lesson017-优化/lesson017.cpp create mode 100644 example/lesson018-优化2/CELLMath.hpp create mode 100644 example/lesson018-优化2/CELLTimestamp.hpp create mode 100644 example/lesson018-优化2/Raster.cpp create mode 100644 example/lesson018-优化2/Raster.h create mode 100644 example/lesson018-优化2/lesson018-优化2.vcproj create mode 100644 example/lesson018-优化2/lesson018.cpp create mode 100644 example/lesson019-优化3/CELLMath.hpp create mode 100644 example/lesson019-优化3/CELLTimestamp.hpp create mode 100644 example/lesson019-优化3/Raster.cpp create mode 100644 example/lesson019-优化3/Raster.h create mode 100644 example/lesson019-优化3/lesson019-优化3.vcproj create mode 100644 example/lesson019-优化3/lesson019.cpp create mode 100644 example/lesson020-优化4/CELLMath.hpp create mode 100644 example/lesson020-优化4/CELLTimestamp.hpp create mode 100644 example/lesson020-优化4/Raster.cpp create mode 100644 example/lesson020-优化4/Raster.h create mode 100644 example/lesson020-优化4/lesson019.cpp create mode 100644 example/lesson020-优化4/lesson020-优化4.vcproj create mode 100644 example/lesson200-drawImage/CELLMath.hpp create mode 100644 example/lesson200-drawImage/CELLTimestamp.hpp create mode 100644 example/lesson200-drawImage/Raster.cpp create mode 100644 example/lesson200-drawImage/Raster.h create mode 100644 example/lesson200-drawImage/lesson200-drawImage.vcproj create mode 100644 example/lesson200-drawImage/lesson200.cpp create mode 100644 example/lesson201-freeImage/CELLMath.hpp create mode 100644 example/lesson201-freeImage/CELLTimestamp.hpp create mode 100644 example/lesson201-freeImage/Raster.cpp create mode 100644 example/lesson201-freeImage/Raster.h create mode 100644 example/lesson201-freeImage/lesson201-freeImage.vcproj create mode 100644 example/lesson201-freeImage/lesson201.cpp create mode 100644 example/lesson202-ImageClass/CELLMath.hpp create mode 100644 example/lesson202-ImageClass/CELLTimestamp.hpp create mode 100644 example/lesson202-ImageClass/Image.hpp create mode 100644 example/lesson202-ImageClass/Raster.cpp create mode 100644 example/lesson202-ImageClass/Raster.h create mode 100644 example/lesson202-ImageClass/lesson202-ImageClass.vcproj create mode 100644 example/lesson202-ImageClass/lesson202.cpp create mode 100644 example/lesson203-color-key/CELLMath.hpp create mode 100644 example/lesson203-color-key/CELLTimestamp.hpp create mode 100644 example/lesson203-color-key/Image.hpp create mode 100644 example/lesson203-color-key/Raster.cpp create mode 100644 example/lesson203-color-key/Raster.h create mode 100644 example/lesson203-color-key/lesson203-color-key.vcproj create mode 100644 example/lesson203-color-key/lesson203.cpp create mode 100644 example/lesson204-alpha-test/CELLMath.hpp create mode 100644 example/lesson204-alpha-test/CELLTimestamp.hpp create mode 100644 example/lesson204-alpha-test/Image.hpp create mode 100644 example/lesson204-alpha-test/Raster.cpp create mode 100644 example/lesson204-alpha-test/Raster.h create mode 100644 example/lesson204-alpha-test/lesson204-alpha-test.vcproj create mode 100644 example/lesson204-alpha-test/lesson204.cpp create mode 100644 example/lesson205-alpha-blend/CELLMath.hpp create mode 100644 example/lesson205-alpha-blend/CELLTimestamp.hpp create mode 100644 example/lesson205-alpha-blend/Image.hpp create mode 100644 example/lesson205-alpha-blend/Raster.cpp create mode 100644 example/lesson205-alpha-blend/Raster.h create mode 100644 example/lesson205-alpha-blend/lesson205-alpha-blend.vcproj create mode 100644 example/lesson205-alpha-blend/lesson205.cpp create mode 100644 example/lesson206-alpha透明/CELLMath.hpp create mode 100644 example/lesson206-alpha透明/CELLTimestamp.hpp create mode 100644 example/lesson206-alpha透明/Image.hpp create mode 100644 example/lesson206-alpha透明/Raster.cpp create mode 100644 example/lesson206-alpha透明/Raster.h create mode 100644 example/lesson206-alpha透明/lesson205.cpp create mode 100644 example/lesson206-alpha透明/lesson206-alpha透明.vcproj create mode 100644 example/lesson206-alpha透明/lesson206.cpp create mode 100644 example/lesson207-drawPart/CELLMath.hpp create mode 100644 example/lesson207-drawPart/CELLTimestamp.hpp create mode 100644 example/lesson207-drawPart/Image.hpp create mode 100644 example/lesson207-drawPart/Raster.cpp create mode 100644 example/lesson207-drawPart/Raster.h create mode 100644 example/lesson207-drawPart/lesson205.cpp create mode 100644 example/lesson207-drawPart/lesson207-drawPart.vcproj create mode 100644 example/lesson207-drawPart/lesson207.cpp create mode 100644 example/lesson208-scale/CELLMath.hpp create mode 100644 example/lesson208-scale/CELLTimestamp.hpp create mode 100644 example/lesson208-scale/Image.hpp create mode 100644 example/lesson208-scale/Raster.cpp create mode 100644 example/lesson208-scale/Raster.h create mode 100644 example/lesson208-scale/lesson205.cpp create mode 100644 example/lesson208-scale/lesson208-scale.vcproj create mode 100644 example/lesson208-scale/lesson208.cpp create mode 100644 example/lesson209-scale-高质量/CELLMath.hpp create mode 100644 example/lesson209-scale-高质量/CELLTimestamp.hpp create mode 100644 example/lesson209-scale-高质量/Image.hpp create mode 100644 example/lesson209-scale-高质量/Raster.cpp create mode 100644 example/lesson209-scale-高质量/Raster.h create mode 100644 example/lesson209-scale-高质量/lesson209-scale-高质量.vcproj create mode 100644 example/lesson209-scale-高质量/lesson209.cpp create mode 100644 example/lesson210-总结/CELLMath.hpp create mode 100644 example/lesson210-总结/CELLTimestamp.hpp create mode 100644 example/lesson210-总结/Image.hpp create mode 100644 example/lesson210-总结/Raster.cpp create mode 100644 example/lesson210-总结/Raster.h create mode 100644 example/lesson210-总结/lesson210-总结.vcproj create mode 100644 example/lesson210-总结/lesson210.cpp create mode 100644 example/lesson300-纹理/CELLMath.hpp create mode 100644 example/lesson300-纹理/CELLTimestamp.hpp create mode 100644 example/lesson300-纹理/Image.hpp create mode 100644 example/lesson300-纹理/Raster.cpp create mode 100644 example/lesson300-纹理/Raster.h create mode 100644 example/lesson300-纹理/lesson300-纹理.vcproj create mode 100644 example/lesson300-纹理/lesson300.cpp create mode 100644 example/lesson301-纹理/CELLMath.hpp create mode 100644 example/lesson301-纹理/CELLTimestamp.hpp create mode 100644 example/lesson301-纹理/Image.hpp create mode 100644 example/lesson301-纹理/Raster.cpp create mode 100644 example/lesson301-纹理/Raster.h create mode 100644 example/lesson301-纹理/lesson301-纹理.vcproj create mode 100644 example/lesson301-纹理/lesson301.cpp create mode 100644 example/lesson302-纹理-color-blend/CELLMath.hpp create mode 100644 example/lesson302-纹理-color-blend/CELLTimestamp.hpp create mode 100644 example/lesson302-纹理-color-blend/Image.hpp create mode 100644 example/lesson302-纹理-color-blend/Raster.cpp create mode 100644 example/lesson302-纹理-color-blend/Raster.h create mode 100644 example/lesson302-纹理-color-blend/lesson302-纹理-color-blend.vcproj create mode 100644 example/lesson302-纹理-color-blend/lesson302.cpp create mode 100644 example/lesson303-状态机/CELLMath.hpp create mode 100644 example/lesson303-状态机/CELLTimestamp.hpp create mode 100644 example/lesson303-状态机/Image.hpp create mode 100644 example/lesson303-状态机/Raster.cpp create mode 100644 example/lesson303-状态机/Raster.h create mode 100644 example/lesson303-状态机/lesson303-状态机.vcproj create mode 100644 example/lesson303-状态机/lesson303.cpp create mode 100644 example/lesson304-状态机2/CELLMath.hpp create mode 100644 example/lesson304-状态机2/CELLTimestamp.hpp create mode 100644 example/lesson304-状态机2/Image.hpp create mode 100644 example/lesson304-状态机2/Raster.cpp create mode 100644 example/lesson304-状态机2/Raster.h create mode 100644 example/lesson304-状态机2/lesson304-状态机2.vcproj create mode 100644 example/lesson304-状态机2/lesson304.cpp create mode 100644 example/lesson305-状态机3/CELLMath.hpp create mode 100644 example/lesson305-状态机3/CELLTimestamp.hpp create mode 100644 example/lesson305-状态机3/Image.hpp create mode 100644 example/lesson305-状态机3/Raster.cpp create mode 100644 example/lesson305-状态机3/Raster.h create mode 100644 example/lesson305-状态机3/lesson305-状态机3.vcproj create mode 100644 example/lesson305-状态机3/lesson305.cpp create mode 100644 example/lesson306-纹理包装-重复/CELLMath.hpp create mode 100644 example/lesson306-纹理包装-重复/CELLTimestamp.hpp create mode 100644 example/lesson306-纹理包装-重复/Image.hpp create mode 100644 example/lesson306-纹理包装-重复/Raster.cpp create mode 100644 example/lesson306-纹理包装-重复/Raster.h create mode 100644 example/lesson306-纹理包装-重复/lesson306-纹理包装-重复.vcproj create mode 100644 example/lesson306-纹理包装-重复/lesson306.cpp create mode 100644 example/lesson307-纹理包装-clamage/CELLMath.hpp create mode 100644 example/lesson307-纹理包装-clamage/CELLTimestamp.hpp create mode 100644 example/lesson307-纹理包装-clamage/Image.hpp create mode 100644 example/lesson307-纹理包装-clamage/Raster.cpp create mode 100644 example/lesson307-纹理包装-clamage/Raster.h create mode 100644 example/lesson307-纹理包装-clamage/lesson307-纹理包装-clamage.vcproj create mode 100644 example/lesson307-纹理包装-clamage/lesson307.cpp create mode 100644 example/lesson308-二维操作矩阵/1.ppt diff --git a/example/lesson001-windows/lesson001-window.vcproj b/example/lesson001-windows/lesson001-window.vcproj new file mode 100644 index 0000000..36a3396 --- /dev/null +++ b/example/lesson001-windows/lesson001-window.vcproj @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson001-windows/window.cpp b/example/lesson001-windows/window.cpp new file mode 100644 index 0000000..9dfc625 --- /dev/null +++ b/example/lesson001-windows/window.cpp @@ -0,0 +1,76 @@ +#include +#include + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowEx( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW | WS_CLIPCHILDREN | WS_CLIPSIBLINGS, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson002-windows/lesson002-windows.vcproj b/example/lesson002-windows/lesson002-windows.vcproj new file mode 100644 index 0000000..421ed6b --- /dev/null +++ b/example/lesson002-windows/lesson002-windows.vcproj @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson002-windows/window2.cpp b/example/lesson002-windows/window2.cpp new file mode 100644 index 0000000..d8911a4 --- /dev/null +++ b/example/lesson002-windows/window2.cpp @@ -0,0 +1,117 @@ +#include +#include + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW | WS_CLIPCHILDREN | WS_CLIPSIBLINGS, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + memset(buffer,0,width * height * 4); + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + + memset(buffer,0,width * height * 4); + //! ޸һ + unsigned char* rgba = (unsigned char*)buffer; + int pitch = width * 4; + + memset(rgba + pitch * 10,255, pitch); + + + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson003-point/CELLMath.hpp b/example/lesson003-point/CELLMath.hpp new file mode 100644 index 0000000..342462f --- /dev/null +++ b/example/lesson003-point/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * theta) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson003-point/Raster.cpp b/example/lesson003-point/Raster.cpp new file mode 100644 index 0000000..d2e7b6b --- /dev/null +++ b/example/lesson003-point/Raster.cpp @@ -0,0 +1,17 @@ + +#include "Raster.h" + +namespace CELL +{ + + + Raster::Raster() + { + } + + Raster::~Raster(void) + { + } + + +} \ No newline at end of file diff --git a/example/lesson003-point/Raster.h b/example/lesson003-point/Raster.h new file mode 100644 index 0000000..b24dca3 --- /dev/null +++ b/example/lesson003-point/Raster.h @@ -0,0 +1,60 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + class Raster + { + public: + Rgba _buffer[256][256]; + public: + Raster(); + ~Raster(void); + + void clear() + { + memset(_buffer,0,sizeof(_buffer)); + } + + void drawPoint(int x,int y, Rgba color,int ptSize) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void setPixel(int x,int y,Rgba color) + { + if (x < 0 || y < 0 || x >= 256 || y >= 256) + { + return; + } + _buffer[y][x] = color; + } + }; +} diff --git a/example/lesson003-point/lesson003-point.vcproj b/example/lesson003-point/lesson003-point.vcproj new file mode 100644 index 0000000..07d6844 --- /dev/null +++ b/example/lesson003-point/lesson003-point.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson003-point/lesson003.cpp b/example/lesson003-point/lesson003.cpp new file mode 100644 index 0000000..fa11700 --- /dev/null +++ b/example/lesson003-point/lesson003.cpp @@ -0,0 +1,118 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_POPUPWINDOW, + 0, + 0, + 256, + 256, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = 256; + bmpInfor.bmiHeader.biHeight = 256; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster; + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + for (int i = 0 ;i < 100 ; ++ i) + { + raster.drawPoint(rand()%256,rand()%256,CELL::Rgba(255,0,0),2); + } + + memcpy(buffer,raster._buffer,sizeof(raster._buffer)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson004-point改进/CELLMath.hpp b/example/lesson004-point改进/CELLMath.hpp new file mode 100644 index 0000000..342462f --- /dev/null +++ b/example/lesson004-point改进/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * theta) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson004-point改进/Raster.cpp b/example/lesson004-point改进/Raster.cpp new file mode 100644 index 0000000..44bf9bc --- /dev/null +++ b/example/lesson004-point改进/Raster.cpp @@ -0,0 +1,54 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + +} \ No newline at end of file diff --git a/example/lesson004-point改进/Raster.h b/example/lesson004-point改进/Raster.h new file mode 100644 index 0000000..1aa24c5 --- /dev/null +++ b/example/lesson004-point改进/Raster.h @@ -0,0 +1,31 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + public: + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson004-point改进/lesson004-point改进.vcproj b/example/lesson004-point改进/lesson004-point改进.vcproj new file mode 100644 index 0000000..fbe6ff0 --- /dev/null +++ b/example/lesson004-point改进/lesson004-point改进.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson004-point改进/lesson004.cpp b/example/lesson004-point改进/lesson004.cpp new file mode 100644 index 0000000..3eed8d8 --- /dev/null +++ b/example/lesson004-point改进/lesson004.cpp @@ -0,0 +1,118 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_POPUPWINDOW, + 0, + 0, + 256, + 256, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + for (int i = 0 ;i < 100 ; ++ i) + { + raster.drawPoint(rand()%256,rand()%256,CELL::Rgba(255,0,0),2); + } + + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson005-line/CELLMath.hpp b/example/lesson005-line/CELLMath.hpp new file mode 100644 index 0000000..342462f --- /dev/null +++ b/example/lesson005-line/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * theta) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson005-line/Raster.cpp b/example/lesson005-line/Raster.cpp new file mode 100644 index 0000000..ad49fe4 --- /dev/null +++ b/example/lesson005-line/Raster.cpp @@ -0,0 +1,86 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0) + { + float slope = yOffset / xOffset; + for (float y = pt1.y; y <= pt2.y ; y += 1.0f) + { + setPixel(pt2.x,y,color); + } + } + else if(yOffset == 0) + { + float slope = yOffset / xOffset; + for (float x = pt1.x; x <= pt2.x ; x += 1.0f) + { + setPixel(x,pt2.y,color); + } + } + else + { + float slope = yOffset / xOffset; + for (float x = pt1.x; x <= pt2.x ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + setPixel(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson005-line/Raster.h b/example/lesson005-line/Raster.h new file mode 100644 index 0000000..6a1258a --- /dev/null +++ b/example/lesson005-line/Raster.h @@ -0,0 +1,33 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + void drawLine(float2 pt1,float2 pt2,Rgba color); + + public: + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson005-line/lesson005-line.vcproj b/example/lesson005-line/lesson005-line.vcproj new file mode 100644 index 0000000..66db3f5 --- /dev/null +++ b/example/lesson005-line/lesson005-line.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson005-line/lesson005.cpp b/example/lesson005-line/lesson005.cpp new file mode 100644 index 0000000..f7382a5 --- /dev/null +++ b/example/lesson005-line/lesson005.cpp @@ -0,0 +1,117 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_POPUPWINDOW, + 0, + 0, + 256, + 256, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + + raster.drawLine(CELL::float2(100,100),CELL::float2(200,200),CELL::Rgba(255,0,0)); + + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson006-line2/CELLMath.hpp b/example/lesson006-line2/CELLMath.hpp new file mode 100644 index 0000000..342462f --- /dev/null +++ b/example/lesson006-line2/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * theta) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson006-line2/Raster.cpp b/example/lesson006-line2/Raster.cpp new file mode 100644 index 0000000..49e5b08 --- /dev/null +++ b/example/lesson006-line2/Raster.cpp @@ -0,0 +1,53 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } +} \ No newline at end of file diff --git a/example/lesson006-line2/Raster.h b/example/lesson006-line2/Raster.h new file mode 100644 index 0000000..2fd0dad --- /dev/null +++ b/example/lesson006-line2/Raster.h @@ -0,0 +1,134 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + void drawLine(float2 pt1,float2 pt2,Rgba color) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color); + } + + if (xOffset == 0) + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float slope = yOffset / xOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + setPixel(pt2.x,y,color); + } + } + else if(yOffset == 0) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + setPixel(x,pt2.y,color); + } + } + else + { + + if (fabs(xOffset) > fabs(yOffset)) + { + float slope = yOffset / xOffset; + + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + setPixel(x,y,color); + } + } + else + { + float slope = xOffset / yOffset; + + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + for (float y = pt1.y; y <= pt2.y ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + setPixel(pt2.x,y,color); + } + } + } + } + + public: + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson006-line2/lesson006-line2.vcproj b/example/lesson006-line2/lesson006-line2.vcproj new file mode 100644 index 0000000..5be3941 --- /dev/null +++ b/example/lesson006-line2/lesson006-line2.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson006-line2/lesson006.cpp b/example/lesson006-line2/lesson006.cpp new file mode 100644 index 0000000..8e97b90 --- /dev/null +++ b/example/lesson006-line2/lesson006.cpp @@ -0,0 +1,117 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_POPUPWINDOW, + 0, + 0, + 256, + 256, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + + raster.drawLine(CELL::float2(-11,111),CELL::float2(666,222),CELL::Rgba(255,0,0)); + + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson007-line-color/CELLMath.hpp b/example/lesson007-line-color/CELLMath.hpp new file mode 100644 index 0000000..342462f --- /dev/null +++ b/example/lesson007-line-color/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * theta) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson007-line-color/Raster.cpp b/example/lesson007-line-color/Raster.cpp new file mode 100644 index 0000000..ca44f29 --- /dev/null +++ b/example/lesson007-line-color/Raster.cpp @@ -0,0 +1,118 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float slope = yOffset / xOffset; + + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float slope = xOffset / yOffset; + + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + for (float y = pt1.y; y <= pt2.y ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(pt2.x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson007-line-color/Raster.h b/example/lesson007-line-color/Raster.h new file mode 100644 index 0000000..5e91f45 --- /dev/null +++ b/example/lesson007-line-color/Raster.h @@ -0,0 +1,33 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + public: + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson007-line-color/lesson007-line-color.vcproj b/example/lesson007-line-color/lesson007-line-color.vcproj new file mode 100644 index 0000000..c7a0e1c --- /dev/null +++ b/example/lesson007-line-color/lesson007-line-color.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson007-line-color/lesson007.cpp b/example/lesson007-line-color/lesson007.cpp new file mode 100644 index 0000000..ee44d77 --- /dev/null +++ b/example/lesson007-line-color/lesson007.cpp @@ -0,0 +1,117 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_POPUPWINDOW, + 0, + 0, + 256, + 256, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + + raster.drawLine(CELL::float2(1,100),CELL::float2(200,33),CELL::Rgba(255,0,0),CELL::Rgba(0,255,0)); + + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson008-lineStrip/CELLMath.hpp b/example/lesson008-lineStrip/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson008-lineStrip/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson008-lineStrip/Raster.cpp b/example/lesson008-lineStrip/Raster.cpp new file mode 100644 index 0000000..f499e87 --- /dev/null +++ b/example/lesson008-lineStrip/Raster.cpp @@ -0,0 +1,116 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson008-lineStrip/Raster.h b/example/lesson008-lineStrip/Raster.h new file mode 100644 index 0000000..9b239ac --- /dev/null +++ b/example/lesson008-lineStrip/Raster.h @@ -0,0 +1,94 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; ++ i) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + + public: + + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson008-lineStrip/lesson008-lineStrip.vcproj b/example/lesson008-lineStrip/lesson008-lineStrip.vcproj new file mode 100644 index 0000000..e6cf70e --- /dev/null +++ b/example/lesson008-lineStrip/lesson008-lineStrip.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson008-lineStrip/lesson008.cpp b/example/lesson008-lineStrip/lesson008.cpp new file mode 100644 index 0000000..9ae505e --- /dev/null +++ b/example/lesson008-lineStrip/lesson008.cpp @@ -0,0 +1,137 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 256, + 256, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 arPoints[] = + { + CELL::float2(11,34), + CELL::float2(33,66), + CELL::float2(1,100), + CELL::float2(22,88), + CELL::float2(100,1), + }; + raster.drawArrays(CELL::DM_LINE_STRIP,arPoints,sizeof(arPoints)/sizeof(arPoints[0])); + + CELL::float2 center(100,100); + + float radius = 80; + + CELL::float2 arCircle[360]; + for (int i = 0 ;i < 360 ; ++ i) + { + float rad = DEG2RAD(i); + arCircle[i].x = radius * cos(rad) + center.x; + arCircle[i].y = radius * sin(rad) + center.y; + } + raster.drawArrays(CELL::DM_LINE_STRIP,arCircle,sizeof(arCircle)/sizeof(arCircle[0])); + + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson009-贝塞尔/CELLMath.hpp b/example/lesson009-贝塞尔/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson009-贝塞尔/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson009-贝塞尔/Raster.cpp b/example/lesson009-贝塞尔/Raster.cpp new file mode 100644 index 0000000..f499e87 --- /dev/null +++ b/example/lesson009-贝塞尔/Raster.cpp @@ -0,0 +1,116 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson009-贝塞尔/Raster.h b/example/lesson009-贝塞尔/Raster.h new file mode 100644 index 0000000..77d3d59 --- /dev/null +++ b/example/lesson009-贝塞尔/Raster.h @@ -0,0 +1,94 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + + public: + + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson009-贝塞尔/lesson009-贝塞尔.vcproj b/example/lesson009-贝塞尔/lesson009-贝塞尔.vcproj new file mode 100644 index 0000000..6ce19fd --- /dev/null +++ b/example/lesson009-贝塞尔/lesson009-贝塞尔.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson009-贝塞尔/lesson009.cpp b/example/lesson009-贝塞尔/lesson009.cpp new file mode 100644 index 0000000..d748487 --- /dev/null +++ b/example/lesson009-贝塞尔/lesson009.cpp @@ -0,0 +1,145 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 points[] = + { + CELL::float2(50,50), + CELL::float2(200,50), + CELL::float2(33,88), + CELL::float2(159,100), + }; + + CELL::float2 prev[2]; + for (float t = 0 ;t < 1.0f ; t += 0.01f) + { + float x = points[0].x * pow(1-t,3) + + 3 * points[1].x * t * pow(1-t,2) + + 3 * points[2].x * t*t * (1-t) + + points[3].x * t * t * t; + float y = points[0].y * pow(1-t,3) + + 3 * points[1].y * t * pow(1-t,2) + + 3 * points[2].y * t*t * (1-t) + + points[3].y * t * t * t; + if (t == 0) + { + prev[0] = CELL::float2(x,y); + } + else + { + prev[1] = CELL::float2(x,y); + raster.drawArrays(CELL::DM_LINES,prev,2); + prev[0] = prev[1]; + } + } + + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson010-rect/CELLMath.hpp b/example/lesson010-rect/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson010-rect/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson010-rect/Raster.cpp b/example/lesson010-rect/Raster.cpp new file mode 100644 index 0000000..73c06a0 --- /dev/null +++ b/example/lesson010-rect/Raster.cpp @@ -0,0 +1,161 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + +} \ No newline at end of file diff --git a/example/lesson010-rect/Raster.h b/example/lesson010-rect/Raster.h new file mode 100644 index 0000000..725a4a2 --- /dev/null +++ b/example/lesson010-rect/Raster.h @@ -0,0 +1,67 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h) + { + for (int x = startX ; x < startX + w ; ++ x) + { + for (int y = startY ; y < startY + h ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + + public: + + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson010-rect/lesson010-rect.vcproj b/example/lesson010-rect/lesson010-rect.vcproj new file mode 100644 index 0000000..d501a94 --- /dev/null +++ b/example/lesson010-rect/lesson010-rect.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson010-rect/lesson010.cpp b/example/lesson010-rect/lesson010.cpp new file mode 100644 index 0000000..80da390 --- /dev/null +++ b/example/lesson010-rect/lesson010.cpp @@ -0,0 +1,147 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 points[] = + { + CELL::float2(50,50), + CELL::float2(200,50), + CELL::float2(33,88), + CELL::float2(159,100), + }; + + CELL::float2 prev[2]; + for (float t = 0 ;t < 1.0f ; t += 0.01f) + { + float x = points[0].x * pow(1-t,3) + + 3 * points[1].x * t * pow(1-t,2) + + 3 * points[2].x * t*t * (1-t) + + points[3].x * t * t * t; + float y = points[0].y * pow(1-t,3) + + 3 * points[1].y * t * pow(1-t,2) + + 3 * points[2].y * t*t * (1-t) + + points[3].y * t * t * t; + if (t == 0) + { + prev[0] = CELL::float2(x,y); + } + else + { + prev[1] = CELL::float2(x,y); + raster.drawArrays(CELL::DM_LINES,prev,2); + prev[0] = prev[1]; + } + } + + + raster.drawFilleRect(-10,-10,80,80); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson011-rectEx/CELLMath.hpp b/example/lesson011-rectEx/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson011-rectEx/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson011-rectEx/Raster.cpp b/example/lesson011-rectEx/Raster.cpp new file mode 100644 index 0000000..73c06a0 --- /dev/null +++ b/example/lesson011-rectEx/Raster.cpp @@ -0,0 +1,161 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + +} \ No newline at end of file diff --git a/example/lesson011-rectEx/Raster.h b/example/lesson011-rectEx/Raster.h new file mode 100644 index 0000000..e58179f --- /dev/null +++ b/example/lesson011-rectEx/Raster.h @@ -0,0 +1,73 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + + public: + + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson011-rectEx/lesson011-rectEx.vcproj b/example/lesson011-rectEx/lesson011-rectEx.vcproj new file mode 100644 index 0000000..160559f --- /dev/null +++ b/example/lesson011-rectEx/lesson011-rectEx.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson011-rectEx/lesson011.cpp b/example/lesson011-rectEx/lesson011.cpp new file mode 100644 index 0000000..afdc299 --- /dev/null +++ b/example/lesson011-rectEx/lesson011.cpp @@ -0,0 +1,147 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 points[] = + { + CELL::float2(50,50), + CELL::float2(200,50), + CELL::float2(33,88), + CELL::float2(159,100), + }; + + CELL::float2 prev[2]; + for (float t = 0 ;t < 1.0f ; t += 0.01f) + { + float x = points[0].x * pow(1-t,3) + + 3 * points[1].x * t * pow(1-t,2) + + 3 * points[2].x * t*t * (1-t) + + points[3].x * t * t * t; + float y = points[0].y * pow(1-t,3) + + 3 * points[1].y * t * pow(1-t,2) + + 3 * points[2].y * t*t * (1-t) + + points[3].y * t * t * t; + if (t == 0) + { + prev[0] = CELL::float2(x,y); + } + else + { + prev[1] = CELL::float2(x,y); + raster.drawArrays(CELL::DM_LINES,prev,2); + prev[0] = prev[1]; + } + } + + + raster.drawFilleRect(300,260,200,200); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson012-rect-color/CELLMath.hpp b/example/lesson012-rect-color/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson012-rect-color/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson012-rect-color/Raster.cpp b/example/lesson012-rect-color/Raster.cpp new file mode 100644 index 0000000..fe43e70 --- /dev/null +++ b/example/lesson012-rect-color/Raster.cpp @@ -0,0 +1,178 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson012-rect-color/Raster.h b/example/lesson012-rect-color/Raster.h new file mode 100644 index 0000000..3819750 --- /dev/null +++ b/example/lesson012-rect-color/Raster.h @@ -0,0 +1,82 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + + public: + + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson012-rect-color/lesson012-rect-color.vcproj b/example/lesson012-rect-color/lesson012-rect-color.vcproj new file mode 100644 index 0000000..67ab8c6 --- /dev/null +++ b/example/lesson012-rect-color/lesson012-rect-color.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson012-rect-color/lesson012.cpp b/example/lesson012-rect-color/lesson012.cpp new file mode 100644 index 0000000..f17250c --- /dev/null +++ b/example/lesson012-rect-color/lesson012.cpp @@ -0,0 +1,164 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 points[] = + { + CELL::float2(50,50), + CELL::float2(200,50), + CELL::float2(33,88), + CELL::float2(159,100), + }; + + CELL::float2 prev[2]; + for (float t = 0 ;t < 1.0f ; t += 0.01f) + { + float x = points[0].x * pow(1-t,3) + + 3 * points[1].x * t * pow(1-t,2) + + 3 * points[2].x * t*t * (1-t) + + points[3].x * t * t * t; + float y = points[0].y * pow(1-t,3) + + 3 * points[1].y * t * pow(1-t,2) + + 3 * points[2].y * t*t * (1-t) + + points[3].y * t * t * t; + if (t == 0) + { + prev[0] = CELL::float2(x,y); + } + else + { + prev[1] = CELL::float2(x,y); + raster.drawArrays(CELL::DM_LINES,prev,2); + prev[0] = prev[1]; + } + } + + + CELL::int2 pt[] = + { + CELL::int2(10,10), + CELL::int2(110,10), + CELL::int2(110,110), + CELL::int2(10,110), + }; + + CELL::Rgba colors[] = + { + CELL::Rgba (255,0,0), + CELL::Rgba (0,255,0), + CELL::Rgba (0,0,255), + CELL::Rgba (255,255,255), + + }; + + raster.drawRect(pt,colors); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson013-triangle/CELLMath.hpp b/example/lesson013-triangle/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson013-triangle/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson013-triangle/Raster.cpp b/example/lesson013-triangle/Raster.cpp new file mode 100644 index 0000000..8d2d9da --- /dev/null +++ b/example/lesson013-triangle/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson013-triangle/Raster.h b/example/lesson013-triangle/Raster.h new file mode 100644 index 0000000..cb16768 --- /dev/null +++ b/example/lesson013-triangle/Raster.h @@ -0,0 +1,69 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + + public: + + void drawLine(int startX,int endX, int y,Rgba color1,Rgba color2) + { + float length = tmax(endX - startX,1); + for (int x = startX ; x <= endX ; ++ x) + { + Rgba color = colorLerp(color1,color2,( x - startX)/length); + setPixel(x,y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson013-triangle/lesson013-triangle.vcproj b/example/lesson013-triangle/lesson013-triangle.vcproj new file mode 100644 index 0000000..402f4f0 --- /dev/null +++ b/example/lesson013-triangle/lesson013-triangle.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson013-triangle/lesson013.cpp b/example/lesson013-triangle/lesson013.cpp new file mode 100644 index 0000000..f17250c --- /dev/null +++ b/example/lesson013-triangle/lesson013.cpp @@ -0,0 +1,164 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 points[] = + { + CELL::float2(50,50), + CELL::float2(200,50), + CELL::float2(33,88), + CELL::float2(159,100), + }; + + CELL::float2 prev[2]; + for (float t = 0 ;t < 1.0f ; t += 0.01f) + { + float x = points[0].x * pow(1-t,3) + + 3 * points[1].x * t * pow(1-t,2) + + 3 * points[2].x * t*t * (1-t) + + points[3].x * t * t * t; + float y = points[0].y * pow(1-t,3) + + 3 * points[1].y * t * pow(1-t,2) + + 3 * points[2].y * t*t * (1-t) + + points[3].y * t * t * t; + if (t == 0) + { + prev[0] = CELL::float2(x,y); + } + else + { + prev[1] = CELL::float2(x,y); + raster.drawArrays(CELL::DM_LINES,prev,2); + prev[0] = prev[1]; + } + } + + + CELL::int2 pt[] = + { + CELL::int2(10,10), + CELL::int2(110,10), + CELL::int2(110,110), + CELL::int2(10,110), + }; + + CELL::Rgba colors[] = + { + CELL::Rgba (255,0,0), + CELL::Rgba (0,255,0), + CELL::Rgba (0,0,255), + CELL::Rgba (255,255,255), + + }; + + raster.drawRect(pt,colors); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson014-span/CELLMath.hpp b/example/lesson014-span/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson014-span/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson014-span/Raster.cpp b/example/lesson014-span/Raster.cpp new file mode 100644 index 0000000..8d2d9da --- /dev/null +++ b/example/lesson014-span/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson014-span/Raster.h b/example/lesson014-span/Raster.h new file mode 100644 index 0000000..5af73b0 --- /dev/null +++ b/example/lesson014-span/Raster.h @@ -0,0 +1,149 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + int _y; + public: + Span(int xStart,int xEnd,int y) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + + int _x2; + int _y2; + Ege(int x1,int y1,int x2,int y2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + + _x2 = x2; + _y2 = y2; + } + else + { + _x1 = x2; + _y1 = y2; + + _x2 = x1; + _y2 = y1; + } + } + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + + public: + + void drawEge(const Ege& e1,const Ege& e2) + { + + float xOffset = e2._x2 - e2._x1; + float yOffset = e2._y2 - e2._y1; + + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float yOffset1 = e1._y2 - e1._y1; + float scale1 = 0; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y <= e2._y2 ; ++ y) + { + int x1 = e1._x1 + scale1 * xOffset1; + int x2 = e2._x1 + scale * xOffset; + + Span span(x1,x2,y); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + for (int x = span._xStart ; x <= span._xEnd; ++ x) + { + setPixel(x,span._y,_color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson014-span/lesson014-span.vcproj b/example/lesson014-span/lesson014-span.vcproj new file mode 100644 index 0000000..d43ea4e --- /dev/null +++ b/example/lesson014-span/lesson014-span.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson014-span/lesson014.cpp b/example/lesson014-span/lesson014.cpp new file mode 100644 index 0000000..f17250c --- /dev/null +++ b/example/lesson014-span/lesson014.cpp @@ -0,0 +1,164 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::float2 points[] = + { + CELL::float2(50,50), + CELL::float2(200,50), + CELL::float2(33,88), + CELL::float2(159,100), + }; + + CELL::float2 prev[2]; + for (float t = 0 ;t < 1.0f ; t += 0.01f) + { + float x = points[0].x * pow(1-t,3) + + 3 * points[1].x * t * pow(1-t,2) + + 3 * points[2].x * t*t * (1-t) + + points[3].x * t * t * t; + float y = points[0].y * pow(1-t,3) + + 3 * points[1].y * t * pow(1-t,2) + + 3 * points[2].y * t*t * (1-t) + + points[3].y * t * t * t; + if (t == 0) + { + prev[0] = CELL::float2(x,y); + } + else + { + prev[1] = CELL::float2(x,y); + raster.drawArrays(CELL::DM_LINES,prev,2); + prev[0] = prev[1]; + } + } + + + CELL::int2 pt[] = + { + CELL::int2(10,10), + CELL::int2(110,10), + CELL::int2(110,110), + CELL::int2(10,110), + }; + + CELL::Rgba colors[] = + { + CELL::Rgba (255,0,0), + CELL::Rgba (0,255,0), + CELL::Rgba (0,0,255), + CELL::Rgba (255,255,255), + + }; + + raster.drawRect(pt,colors); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson015-span/CELLMath.hpp b/example/lesson015-span/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson015-span/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson015-span/Raster.cpp b/example/lesson015-span/Raster.cpp new file mode 100644 index 0000000..8d2d9da --- /dev/null +++ b/example/lesson015-span/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson015-span/Raster.h b/example/lesson015-span/Raster.h new file mode 100644 index 0000000..b7b8ba1 --- /dev/null +++ b/example/lesson015-span/Raster.h @@ -0,0 +1,183 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + int _y; + public: + Span(int xStart,int xEnd,int y) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + + int _x2; + int _y2; + Ege(int x1,int y1,int x2,int y2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + + _x2 = x2; + _y2 = y2; + } + else + { + _x1 = x2; + _y1 = y2; + + _x2 = x1; + _y2 = y1; + } + } + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,p1.x,p1.y), + Ege(p1.x,p1.y,p2.x,p2.y), + Ege(p2.x,p2.y,p0.x,p0.y), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + + Span span(x1,x2,y); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + setPixel(x,span._y,_color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson015-span/lesson015-span.vcproj b/example/lesson015-span/lesson015-span.vcproj new file mode 100644 index 0000000..500d69a --- /dev/null +++ b/example/lesson015-span/lesson015-span.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson015-span/lesson015.cpp b/example/lesson015-span/lesson015.cpp new file mode 100644 index 0000000..53ce00a --- /dev/null +++ b/example/lesson015-span/lesson015.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + raster.drawTriangle(pt[0],pt[1],pt[2]); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson016-triangle-color/CELLMath.hpp b/example/lesson016-triangle-color/CELLMath.hpp new file mode 100644 index 0000000..35feefa --- /dev/null +++ b/example/lesson016-triangle-color/CELLMath.hpp @@ -0,0 +1,5930 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson016-triangle-color/Raster.cpp b/example/lesson016-triangle-color/Raster.cpp new file mode 100644 index 0000000..8d2d9da --- /dev/null +++ b/example/lesson016-triangle-color/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (Rgba*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson016-triangle-color/Raster.h b/example/lesson016-triangle-color/Raster.h new file mode 100644 index 0000000..f59e891 --- /dev/null +++ b/example/lesson016-triangle-color/Raster.h @@ -0,0 +1,204 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + Rgba* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,(float)(x - span._xStart)/length + ); + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color; + } + }; +} diff --git a/example/lesson016-triangle-color/lesson016-triangle-color.vcproj b/example/lesson016-triangle-color/lesson016-triangle-color.vcproj new file mode 100644 index 0000000..1242644 --- /dev/null +++ b/example/lesson016-triangle-color/lesson016-triangle-color.vcproj @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson016-triangle-color/lesson016.cpp b/example/lesson016-triangle-color/lesson016.cpp new file mode 100644 index 0000000..79d2ac5 --- /dev/null +++ b/example/lesson016-triangle-color/lesson016.cpp @@ -0,0 +1,126 @@ +#include +#include + +#include "Raster.h" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson017-优化/CELLMath.hpp b/example/lesson017-优化/CELLMath.hpp new file mode 100644 index 0000000..cd5e9b8 --- /dev/null +++ b/example/lesson017-优化/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson017-优化/CELLTimestamp.hpp b/example/lesson017-优化/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson017-优化/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson017-优化/Raster.cpp b/example/lesson017-优化/Raster.cpp new file mode 100644 index 0000000..b5b15e2 --- /dev/null +++ b/example/lesson017-优化/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson017-优化/Raster.h b/example/lesson017-优化/Raster.h new file mode 100644 index 0000000..4ad5c92 --- /dev/null +++ b/example/lesson017-优化/Raster.h @@ -0,0 +1,208 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson017-优化/lesson017-优化.vcproj b/example/lesson017-优化/lesson017-优化.vcproj new file mode 100644 index 0000000..382a529 --- /dev/null +++ b/example/lesson017-优化/lesson017-优化.vcproj @@ -0,0 +1,211 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson017-优化/lesson017.cpp b/example/lesson017-优化/lesson017.cpp new file mode 100644 index 0000000..a7232c1 --- /dev/null +++ b/example/lesson017-优化/lesson017.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson018-优化2/CELLMath.hpp b/example/lesson018-优化2/CELLMath.hpp new file mode 100644 index 0000000..cd5e9b8 --- /dev/null +++ b/example/lesson018-优化2/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson018-优化2/CELLTimestamp.hpp b/example/lesson018-优化2/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson018-优化2/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson018-优化2/Raster.cpp b/example/lesson018-优化2/Raster.cpp new file mode 100644 index 0000000..b5b15e2 --- /dev/null +++ b/example/lesson018-优化2/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson018-优化2/Raster.h b/example/lesson018-优化2/Raster.h new file mode 100644 index 0000000..b26b9a2 --- /dev/null +++ b/example/lesson018-优化2/Raster.h @@ -0,0 +1,222 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + public: + + bool isInRect(int2 pt) + { + if (pt.x >= 0 && pt.x <= _width && pt.y >= 0 && pt.y <= _height) + { + return true; + } + return false; + } + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { +// if ((!isInRect(p0)) && (!isInRect(p1)) && (!isInRect(p2))) +// { +// return; +// } + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson018-优化2/lesson018-优化2.vcproj b/example/lesson018-优化2/lesson018-优化2.vcproj new file mode 100644 index 0000000..2b51430 --- /dev/null +++ b/example/lesson018-优化2/lesson018-优化2.vcproj @@ -0,0 +1,211 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson018-优化2/lesson018.cpp b/example/lesson018-优化2/lesson018.cpp new file mode 100644 index 0000000..940dc51 --- /dev/null +++ b/example/lesson018-优化2/lesson018.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(-100,-10), + CELL::int2(-10,-100), + CELL::int2(-200,-100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson019-优化3/CELLMath.hpp b/example/lesson019-优化3/CELLMath.hpp new file mode 100644 index 0000000..cd5e9b8 --- /dev/null +++ b/example/lesson019-优化3/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson019-优化3/CELLTimestamp.hpp b/example/lesson019-优化3/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson019-优化3/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson019-优化3/Raster.cpp b/example/lesson019-优化3/Raster.cpp new file mode 100644 index 0000000..b5b15e2 --- /dev/null +++ b/example/lesson019-优化3/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson019-优化3/Raster.h b/example/lesson019-优化3/Raster.h new file mode 100644 index 0000000..cb4efd5 --- /dev/null +++ b/example/lesson019-优化3/Raster.h @@ -0,0 +1,229 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + public: + + bool isInRect(int2 pt) + { + if (pt.x >= 0 && pt.x <= _width && pt.y >= 0 && pt.y <= _height) + { + return true; + } + return false; + } + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { +// if ((!isInRect(p0)) && (!isInRect(p1)) && (!isInRect(p2))) +// { +// return; +// } + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + int startY = tmax(e2._y1,0); + int endY = tmin(e2._y2,_height); + scale += (startY - e2._y1)/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + int startY1 = tmax(e1._y1,0); + int endY1 = tmin(e1._y2,_height); + scale1 += (startY1 - e1._y1)/yOffset1; + + for (int y = startY ; y < endY ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson019-优化3/lesson019-优化3.vcproj b/example/lesson019-优化3/lesson019-优化3.vcproj new file mode 100644 index 0000000..1a3c996 --- /dev/null +++ b/example/lesson019-优化3/lesson019-优化3.vcproj @@ -0,0 +1,211 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson019-优化3/lesson019.cpp b/example/lesson019-优化3/lesson019.cpp new file mode 100644 index 0000000..0497fd5 --- /dev/null +++ b/example/lesson019-优化3/lesson019.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(230,-80), + CELL::int2(10,280), + CELL::int2(600,280), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson020-优化4/CELLMath.hpp b/example/lesson020-优化4/CELLMath.hpp new file mode 100644 index 0000000..cd5e9b8 --- /dev/null +++ b/example/lesson020-优化4/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson020-优化4/CELLTimestamp.hpp b/example/lesson020-优化4/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson020-优化4/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson020-优化4/Raster.cpp b/example/lesson020-优化4/Raster.cpp new file mode 100644 index 0000000..b5b15e2 --- /dev/null +++ b/example/lesson020-优化4/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson020-优化4/Raster.h b/example/lesson020-优化4/Raster.h new file mode 100644 index 0000000..f992c26 --- /dev/null +++ b/example/lesson020-优化4/Raster.h @@ -0,0 +1,232 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + public: + + bool isInRect(int2 pt) + { + if (pt.x >= 0 && pt.x <= _width && pt.y >= 0 && pt.y <= _height) + { + return true; + } + return false; + } + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { +// if ((!isInRect(p0)) && (!isInRect(p1)) && (!isInRect(p2))) +// { +// return; +// } + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + int startY = tmax(e2._y1,0); + int endY = tmin(e2._y2,_height); + scale += (startY - e2._y1)/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + int startY1 = tmax(e1._y1,0); + int endY1 = tmin(e1._y2,_height); + scale1 += (startY1 - e1._y1)/yOffset1; + + for (int y = startY ; y < endY ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + int startX = tmax(span._xStart,0); + int endX = tmin(span._xEnd,_width); + scale += (startX - span._xStart)/length; + for (int x = startX ; x < endX; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixelEx(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson020-优化4/lesson019.cpp b/example/lesson020-优化4/lesson019.cpp new file mode 100644 index 0000000..0497fd5 --- /dev/null +++ b/example/lesson020-优化4/lesson019.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(230,-80), + CELL::int2(10,280), + CELL::int2(600,280), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson020-优化4/lesson020-优化4.vcproj b/example/lesson020-优化4/lesson020-优化4.vcproj new file mode 100644 index 0000000..0a225bc --- /dev/null +++ b/example/lesson020-优化4/lesson020-优化4.vcproj @@ -0,0 +1,211 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson200-drawImage/CELLMath.hpp b/example/lesson200-drawImage/CELLMath.hpp new file mode 100644 index 0000000..cd5e9b8 --- /dev/null +++ b/example/lesson200-drawImage/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson200-drawImage/CELLTimestamp.hpp b/example/lesson200-drawImage/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson200-drawImage/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson200-drawImage/Raster.cpp b/example/lesson200-drawImage/Raster.cpp new file mode 100644 index 0000000..b5b15e2 --- /dev/null +++ b/example/lesson200-drawImage/Raster.cpp @@ -0,0 +1,202 @@ + +#include "Raster.h" + +namespace CELL +{ + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson200-drawImage/Raster.h b/example/lesson200-drawImage/Raster.h new file mode 100644 index 0000000..b1a3855 --- /dev/null +++ b/example/lesson200-drawImage/Raster.h @@ -0,0 +1,226 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,int w,int h) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color(rand()%256,rand()%256,rand()%256); + setPixelEx(x,y,color); + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson200-drawImage/lesson200-drawImage.vcproj b/example/lesson200-drawImage/lesson200-drawImage.vcproj new file mode 100644 index 0000000..2683955 --- /dev/null +++ b/example/lesson200-drawImage/lesson200-drawImage.vcproj @@ -0,0 +1,210 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson200-drawImage/lesson200.cpp b/example/lesson200-drawImage/lesson200.cpp new file mode 100644 index 0000000..9951f0c --- /dev/null +++ b/example/lesson200-drawImage/lesson200.cpp @@ -0,0 +1,144 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(100,100,200,200); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson201-freeImage/CELLMath.hpp b/example/lesson201-freeImage/CELLMath.hpp new file mode 100644 index 0000000..cd5e9b8 --- /dev/null +++ b/example/lesson201-freeImage/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson201-freeImage/CELLTimestamp.hpp b/example/lesson201-freeImage/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson201-freeImage/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson201-freeImage/Raster.cpp b/example/lesson201-freeImage/Raster.cpp new file mode 100644 index 0000000..1339d1c --- /dev/null +++ b/example/lesson201-freeImage/Raster.cpp @@ -0,0 +1,230 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + + unsigned loadImage(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + + FreeImage_Unload(dib); + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson201-freeImage/Raster.h b/example/lesson201-freeImage/Raster.h new file mode 100644 index 0000000..b1a3855 --- /dev/null +++ b/example/lesson201-freeImage/Raster.h @@ -0,0 +1,226 @@ +#pragma once + +#include "CELLMath.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,int w,int h) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color(rand()%256,rand()%256,rand()%256); + setPixelEx(x,y,color); + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson201-freeImage/lesson201-freeImage.vcproj b/example/lesson201-freeImage/lesson201-freeImage.vcproj new file mode 100644 index 0000000..5bac2d8 --- /dev/null +++ b/example/lesson201-freeImage/lesson201-freeImage.vcproj @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson201-freeImage/lesson201.cpp b/example/lesson201-freeImage/lesson201.cpp new file mode 100644 index 0000000..9951f0c --- /dev/null +++ b/example/lesson201-freeImage/lesson201.cpp @@ -0,0 +1,144 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(100,100,200,200); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + return 0; +} \ No newline at end of file diff --git a/example/lesson202-ImageClass/CELLMath.hpp b/example/lesson202-ImageClass/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson202-ImageClass/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson202-ImageClass/CELLTimestamp.hpp b/example/lesson202-ImageClass/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson202-ImageClass/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson202-ImageClass/Image.hpp b/example/lesson202-ImageClass/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson202-ImageClass/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson202-ImageClass/Raster.cpp b/example/lesson202-ImageClass/Raster.cpp new file mode 100644 index 0000000..aeafcb7 --- /dev/null +++ b/example/lesson202-ImageClass/Raster.cpp @@ -0,0 +1,231 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson202-ImageClass/Raster.h b/example/lesson202-ImageClass/Raster.h new file mode 100644 index 0000000..eb79aa3 --- /dev/null +++ b/example/lesson202-ImageClass/Raster.h @@ -0,0 +1,228 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson202-ImageClass/lesson202-ImageClass.vcproj b/example/lesson202-ImageClass/lesson202-ImageClass.vcproj new file mode 100644 index 0000000..2b15762 --- /dev/null +++ b/example/lesson202-ImageClass/lesson202-ImageClass.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson202-ImageClass/lesson202.cpp b/example/lesson202-ImageClass/lesson202.cpp new file mode 100644 index 0000000..120bcf0 --- /dev/null +++ b/example/lesson202-ImageClass/lesson202.cpp @@ -0,0 +1,163 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 480, + 320, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/1.jpg",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(100,100,image); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson203-color-key/CELLMath.hpp b/example/lesson203-color-key/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson203-color-key/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson203-color-key/CELLTimestamp.hpp b/example/lesson203-color-key/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson203-color-key/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson203-color-key/Image.hpp b/example/lesson203-color-key/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson203-color-key/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson203-color-key/Raster.cpp b/example/lesson203-color-key/Raster.cpp new file mode 100644 index 0000000..786b569 --- /dev/null +++ b/example/lesson203-color-key/Raster.cpp @@ -0,0 +1,283 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + +} \ No newline at end of file diff --git a/example/lesson203-color-key/Raster.h b/example/lesson203-color-key/Raster.h new file mode 100644 index 0000000..45ace6c --- /dev/null +++ b/example/lesson203-color-key/Raster.h @@ -0,0 +1,214 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson203-color-key/lesson203-color-key.vcproj b/example/lesson203-color-key/lesson203-color-key.vcproj new file mode 100644 index 0000000..99fbb20 --- /dev/null +++ b/example/lesson203-color-key/lesson203-color-key.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson203-color-key/lesson203.cpp b/example/lesson203-color-key/lesson203.cpp new file mode 100644 index 0000000..1b89e28 --- /dev/null +++ b/example/lesson203-color-key/lesson203.cpp @@ -0,0 +1,166 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/colorKey.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageWidthColorKey(100,100,colorKey,CELL::Rgba(255,0,0)); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson204-alpha-test/CELLMath.hpp b/example/lesson204-alpha-test/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson204-alpha-test/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson204-alpha-test/CELLTimestamp.hpp b/example/lesson204-alpha-test/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson204-alpha-test/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson204-alpha-test/Image.hpp b/example/lesson204-alpha-test/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson204-alpha-test/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson204-alpha-test/Raster.cpp b/example/lesson204-alpha-test/Raster.cpp new file mode 100644 index 0000000..786b569 --- /dev/null +++ b/example/lesson204-alpha-test/Raster.cpp @@ -0,0 +1,283 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + +} \ No newline at end of file diff --git a/example/lesson204-alpha-test/Raster.h b/example/lesson204-alpha-test/Raster.h new file mode 100644 index 0000000..d5f165f --- /dev/null +++ b/example/lesson204-alpha-test/Raster.h @@ -0,0 +1,235 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a < alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson204-alpha-test/lesson204-alpha-test.vcproj b/example/lesson204-alpha-test/lesson204-alpha-test.vcproj new file mode 100644 index 0000000..745c5b1 --- /dev/null +++ b/example/lesson204-alpha-test/lesson204-alpha-test.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson204-alpha-test/lesson204.cpp b/example/lesson204-alpha-test/lesson204.cpp new file mode 100644 index 0000000..6fe9a4e --- /dev/null +++ b/example/lesson204-alpha-test/lesson204.cpp @@ -0,0 +1,166 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson205-alpha-blend/CELLMath.hpp b/example/lesson205-alpha-blend/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson205-alpha-blend/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson205-alpha-blend/CELLTimestamp.hpp b/example/lesson205-alpha-blend/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson205-alpha-blend/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson205-alpha-blend/Image.hpp b/example/lesson205-alpha-blend/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson205-alpha-blend/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson205-alpha-blend/Raster.cpp b/example/lesson205-alpha-blend/Raster.cpp new file mode 100644 index 0000000..a6070ce --- /dev/null +++ b/example/lesson205-alpha-blend/Raster.cpp @@ -0,0 +1,324 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaTest( int startX,int startY,const Image* image,byte alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a > alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaBlend( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,srcColor._a/255.0f); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson205-alpha-blend/Raster.h b/example/lesson205-alpha-blend/Raster.h new file mode 100644 index 0000000..b22cbce --- /dev/null +++ b/example/lesson205-alpha-blend/Raster.h @@ -0,0 +1,222 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha); + + void drawImageAlphaBlend(int startX,int startY,const Image* image); + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson205-alpha-blend/lesson205-alpha-blend.vcproj b/example/lesson205-alpha-blend/lesson205-alpha-blend.vcproj new file mode 100644 index 0000000..98b764a --- /dev/null +++ b/example/lesson205-alpha-blend/lesson205-alpha-blend.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson205-alpha-blend/lesson205.cpp b/example/lesson205-alpha-blend/lesson205.cpp new file mode 100644 index 0000000..5724d4d --- /dev/null +++ b/example/lesson205-alpha-blend/lesson205.cpp @@ -0,0 +1,167 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + raster.drawImageAlphaBlend(250,100,colorKey); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson206-alpha透明/CELLMath.hpp b/example/lesson206-alpha透明/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson206-alpha透明/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson206-alpha透明/CELLTimestamp.hpp b/example/lesson206-alpha透明/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson206-alpha透明/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson206-alpha透明/Image.hpp b/example/lesson206-alpha透明/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson206-alpha透明/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson206-alpha透明/Raster.cpp b/example/lesson206-alpha透明/Raster.cpp new file mode 100644 index 0000000..4d4e5f2 --- /dev/null +++ b/example/lesson206-alpha透明/Raster.cpp @@ -0,0 +1,323 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaTest( int startX,int startY,const Image* image,byte alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a > alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaBlend( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,srcColor._a/255.0f * alpha); + setPixelEx(x,y,color); + } + } + } +} \ No newline at end of file diff --git a/example/lesson206-alpha透明/Raster.h b/example/lesson206-alpha透明/Raster.h new file mode 100644 index 0000000..f006512 --- /dev/null +++ b/example/lesson206-alpha透明/Raster.h @@ -0,0 +1,244 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha); + + void drawImageAlphaBlend(int startX,int startY,const Image* image,float alpha); + + + void drawImageAlpha( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,alpha); + setPixelEx(x,y,color); + } + } + } + + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson206-alpha透明/lesson205.cpp b/example/lesson206-alpha透明/lesson205.cpp new file mode 100644 index 0000000..5724d4d --- /dev/null +++ b/example/lesson206-alpha透明/lesson205.cpp @@ -0,0 +1,167 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + raster.drawImageAlphaBlend(250,100,colorKey); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson206-alpha透明/lesson206-alpha透明.vcproj b/example/lesson206-alpha透明/lesson206-alpha透明.vcproj new file mode 100644 index 0000000..5e9f8be --- /dev/null +++ b/example/lesson206-alpha透明/lesson206-alpha透明.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson206-alpha透明/lesson206.cpp b/example/lesson206-alpha透明/lesson206.cpp new file mode 100644 index 0000000..c818dde --- /dev/null +++ b/example/lesson206-alpha透明/lesson206.cpp @@ -0,0 +1,169 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + raster.drawImageAlphaBlend(250,100,colorKey,0.3); + + //raster.drawImageAlpha(250,200,colorKey,0.5f); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson207-drawPart/CELLMath.hpp b/example/lesson207-drawPart/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson207-drawPart/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson207-drawPart/CELLTimestamp.hpp b/example/lesson207-drawPart/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson207-drawPart/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson207-drawPart/Image.hpp b/example/lesson207-drawPart/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson207-drawPart/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson207-drawPart/Raster.cpp b/example/lesson207-drawPart/Raster.cpp new file mode 100644 index 0000000..f0d7b79 --- /dev/null +++ b/example/lesson207-drawPart/Raster.cpp @@ -0,0 +1,344 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaTest( int startX,int startY,const Image* image,byte alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a > alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaBlend( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,srcColor._a/255.0f * alpha); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageAlpha( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,alpha); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson207-drawPart/Raster.h b/example/lesson207-drawPart/Raster.h new file mode 100644 index 0000000..712492c --- /dev/null +++ b/example/lesson207-drawPart/Raster.h @@ -0,0 +1,244 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha); + + void drawImageAlphaBlend(int startX,int startY,const Image* image,float alpha); + + + void drawImageAlpha( int startX,int startY,const Image* image,float alpha ); + + void drawImage(int startX,int startY,const Image* image,int x1,int y1,int w,int h) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left + x1,y - top + y1); + setPixelEx(x,y,srcColor); + } + } + } + + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson207-drawPart/lesson205.cpp b/example/lesson207-drawPart/lesson205.cpp new file mode 100644 index 0000000..5724d4d --- /dev/null +++ b/example/lesson207-drawPart/lesson205.cpp @@ -0,0 +1,167 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + raster.drawImageAlphaBlend(250,100,colorKey); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson207-drawPart/lesson207-drawPart.vcproj b/example/lesson207-drawPart/lesson207-drawPart.vcproj new file mode 100644 index 0000000..f6dc995 --- /dev/null +++ b/example/lesson207-drawPart/lesson207-drawPart.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson207-drawPart/lesson207.cpp b/example/lesson207-drawPart/lesson207.cpp new file mode 100644 index 0000000..cb8e46f --- /dev/null +++ b/example/lesson207-drawPart/lesson207.cpp @@ -0,0 +1,178 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + raster.drawImageAlphaBlend(250,100,colorKey,0.3); + + //raster.drawImageAlpha(250,200,colorKey,0.5f); + + raster.drawImage(200,300,image1,50,50,30,50); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + delete colorKey; + + return 0; +} \ No newline at end of file diff --git a/example/lesson208-scale/CELLMath.hpp b/example/lesson208-scale/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson208-scale/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson208-scale/CELLTimestamp.hpp b/example/lesson208-scale/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson208-scale/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson208-scale/Image.hpp b/example/lesson208-scale/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson208-scale/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson208-scale/Raster.cpp b/example/lesson208-scale/Raster.cpp new file mode 100644 index 0000000..f706dca --- /dev/null +++ b/example/lesson208-scale/Raster.cpp @@ -0,0 +1,362 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image,int x1,int y1,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left + x1,y - top + y1); + setPixelEx(x,y,srcColor); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaTest( int startX,int startY,const Image* image,byte alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a > alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaBlend( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,srcColor._a/255.0f * alpha); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageAlpha( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,alpha); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson208-scale/Raster.h b/example/lesson208-scale/Raster.h new file mode 100644 index 0000000..52e025f --- /dev/null +++ b/example/lesson208-scale/Raster.h @@ -0,0 +1,246 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha); + + void drawImageAlphaBlend(int startX,int startY,const Image* image,float alpha); + + + void drawImageAlpha( int startX,int startY,const Image* image,float alpha ); + + void drawImage(int startX,int startY,const Image* image,int x1,int y1,int w,int h); + + + void drawImageScale(int dstX,int dstY,int dstW,int dstH,const Image* image) + { + float xScale = (float)image->width()/(float)dstW; + float yScale = (float)image->height()/(float)dstH; + + for (int x = dstX ;x pixelAt(xx,yy); + setPixelEx(x,y,srcColor); + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson208-scale/lesson205.cpp b/example/lesson208-scale/lesson205.cpp new file mode 100644 index 0000000..5724d4d --- /dev/null +++ b/example/lesson208-scale/lesson205.cpp @@ -0,0 +1,167 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + raster.drawImageAlphaTest(100,100,colorKey,100); + raster.drawImageAlphaBlend(250,100,colorKey); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + + return 0; +} \ No newline at end of file diff --git a/example/lesson208-scale/lesson208-scale.vcproj b/example/lesson208-scale/lesson208-scale.vcproj new file mode 100644 index 0000000..95b2907 --- /dev/null +++ b/example/lesson208-scale/lesson208-scale.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson208-scale/lesson208.cpp b/example/lesson208-scale/lesson208.cpp new file mode 100644 index 0000000..d9f8312 --- /dev/null +++ b/example/lesson208-scale/lesson208.cpp @@ -0,0 +1,181 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + +// raster.drawImageAlphaTest(100,100,colorKey,100); +// raster.drawImageAlphaBlend(250,100,colorKey,0.3); +// +// //raster.drawImageAlpha(250,200,colorKey,0.5f); +// +// raster.drawImage(200,300,image1,50,50,30,50); + + raster.drawImageScale(100,100,250,100,image1); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + delete colorKey; + + return 0; +} \ No newline at end of file diff --git a/example/lesson209-scale-高质量/CELLMath.hpp b/example/lesson209-scale-高质量/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson209-scale-高质量/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson209-scale-高质量/CELLTimestamp.hpp b/example/lesson209-scale-高质量/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson209-scale-高质量/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson209-scale-高质量/Image.hpp b/example/lesson209-scale-高质量/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson209-scale-高质量/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson209-scale-高质量/Raster.cpp b/example/lesson209-scale-高质量/Raster.cpp new file mode 100644 index 0000000..f706dca --- /dev/null +++ b/example/lesson209-scale-高质量/Raster.cpp @@ -0,0 +1,362 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image,int x1,int y1,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left + x1,y - top + y1); + setPixelEx(x,y,srcColor); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaTest( int startX,int startY,const Image* image,byte alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a > alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaBlend( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,srcColor._a/255.0f * alpha); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageAlpha( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,alpha); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson209-scale-高质量/Raster.h b/example/lesson209-scale-高质量/Raster.h new file mode 100644 index 0000000..52e025f --- /dev/null +++ b/example/lesson209-scale-高质量/Raster.h @@ -0,0 +1,246 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha); + + void drawImageAlphaBlend(int startX,int startY,const Image* image,float alpha); + + + void drawImageAlpha( int startX,int startY,const Image* image,float alpha ); + + void drawImage(int startX,int startY,const Image* image,int x1,int y1,int w,int h); + + + void drawImageScale(int dstX,int dstY,int dstW,int dstH,const Image* image) + { + float xScale = (float)image->width()/(float)dstW; + float yScale = (float)image->height()/(float)dstH; + + for (int x = dstX ;x pixelAt(xx,yy); + setPixelEx(x,y,srcColor); + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson209-scale-高质量/lesson209-scale-高质量.vcproj b/example/lesson209-scale-高质量/lesson209-scale-高质量.vcproj new file mode 100644 index 0000000..5e04119 --- /dev/null +++ b/example/lesson209-scale-高质量/lesson209-scale-高质量.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson209-scale-高质量/lesson209.cpp b/example/lesson209-scale-高质量/lesson209.cpp new file mode 100644 index 0000000..d9f8312 --- /dev/null +++ b/example/lesson209-scale-高质量/lesson209.cpp @@ -0,0 +1,181 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + +// raster.drawImageAlphaTest(100,100,colorKey,100); +// raster.drawImageAlphaBlend(250,100,colorKey,0.3); +// +// //raster.drawImageAlpha(250,200,colorKey,0.5f); +// +// raster.drawImage(200,300,image1,50,50,30,50); + + raster.drawImageScale(100,100,250,100,image1); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + delete colorKey; + + return 0; +} \ No newline at end of file diff --git a/example/lesson210-总结/CELLMath.hpp b/example/lesson210-总结/CELLMath.hpp new file mode 100644 index 0000000..881ac50 --- /dev/null +++ b/example/lesson210-总结/CELLMath.hpp @@ -0,0 +1,5942 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson210-总结/CELLTimestamp.hpp b/example/lesson210-总结/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson210-总结/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson210-总结/Image.hpp b/example/lesson210-总结/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson210-总结/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson210-总结/Raster.cpp b/example/lesson210-总结/Raster.cpp new file mode 100644 index 0000000..f706dca --- /dev/null +++ b/example/lesson210-总结/Raster.cpp @@ -0,0 +1,362 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } + + void Raster::drawPoint( int x,int y, Rgba color,int ptSize ) + { + switch(ptSize) + { + case 1: + setPixel(x,y,color); + break; + case 2: + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + break; + case 3: + + setPixel(x - 1, y - 1,color); + setPixel(x + 0, y - 1,color); + setPixel(x + 1, y - 1,color); + + setPixel(x - 1, y + 0,color); + setPixel(x + 0, y + 0,color); + setPixel(x + 1, y + 0,color); + + setPixel(x - 1, y + 1,color); + setPixel(x + 0, y + 1,color); + setPixel(x + 1, y + 1,color); + + break; + } + } + + void Raster::drawLine( float2 pt1,float2 pt2,Rgba color1,Rgba color2 ) + { + float xOffset = pt1.x - pt2.x; + float yOffset = pt1.y - pt2.y; + + if (xOffset == 0 && yOffset == 0) + { + setPixel(pt1.x,pt1.y,color1); + } + + if (fabs(xOffset) > fabs(yOffset)) + { + float xMin; + float xMax; + if (pt1.x < pt2.x) + { + xMin = pt1.x; + xMax = pt2.x; + } + else + { + xMin = pt2.x; + xMax = pt1.x; + } + + float lenth = xMax - xMin; + float slope = yOffset / xOffset; + for (float x = xMin; x <= xMax ; x += 1.0f) + { + float y = pt1.y + (x - pt1.x) * slope; + float scaler = (x - xMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + else + { + float yMin; + float yMax; + if (pt1.y < pt2.y) + { + yMin = pt1.y; + yMax = pt2.y; + } + else + { + yMin = pt2.y; + yMax = pt1.y; + } + + float lenth = yMax - yMin; + float slope = xOffset / yOffset; + for (float y = yMin; y <= yMax ; y += 1.0f) + { + float x = pt1.x + (y - pt1.y) * slope; + float scaler = (y - yMin)/lenth; + Rgba color = colorLerp(color1,color2,scaler); + setPixel(x,y,color); + } + } + } + + void Raster::drawArrays( DRAWMODE mode,const float2* points,int count ) + { + switch (mode) + { + case DM_POINTS: + { + for (int i = 0 ;i < count ; ++ i) + { + drawPoints(points[i],_color); + } + } + break; + case DM_LINES: + { + count = count/2 * 2; + for (int i = 0 ;i < count ; i += 2) + { + drawLine(points[i],points[i + 1],_color,_color); + } + } + break; + case DM_LINE_LOOP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + drawLine(points[0],points[count - 1],_color,_color); + } + break; + case DM_LINE_STRIP: + { + drawLine(points[0],points[1],_color,_color); + for (int i = 2 ;i < count ; ++ i) + { + drawLine(points[i - 1],points[i],_color,_color); + } + } + break; + default: + break; + } + } + + void Raster::drawFilleRect( int startX,int startY,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + setPixelEx(x,y,_color); + } + } + } + + void Raster::drawRect( const int2* points,const Rgba* colors ) + { + int left = tmax(points[0].x,0); + int top = tmax(points[0].y,0); + + int right = tmin(points[2].x,_width); + int bottom = tmin(points[2].y,_height); + + float w = right - left; + float h = bottom - top; + + for (int x = left ; x < right ; ++ x) + { + Rgba color1 = colorLerp(colors[0],colors[1],(x - left)/w); + Rgba color2 = colorLerp(colors[3],colors[2],(x - left)/w); + + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = colorLerp(color1,color2,(y - top)/h); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImage( int startX,int startY,const Image* image,int x1,int y1,int w,int h ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + w,_width); + int bottom = tmin(startY + h,_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left + x1,y - top + y1); + setPixelEx(x,y,srcColor); + } + } + } + + void Raster::drawImageWidthColorKey( int startX,int startY,const Image* image,Rgba key ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color != key) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaTest( int startX,int startY,const Image* image,byte alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + if (color._a > alpha) + { + setPixelEx(x,y,color); + } + } + } + } + + void Raster::drawImageAlphaBlend( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,srcColor._a/255.0f * alpha); + setPixelEx(x,y,color); + } + } + } + + void Raster::drawImageAlpha( int startX,int startY,const Image* image,float alpha ) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba srcColor = image->pixelAt(x - left,y - top); + Rgba dstColor = getPixel(x,y); + Rgba color = colorLerp(dstColor,srcColor,alpha); + setPixelEx(x,y,color); + } + } + } + +} \ No newline at end of file diff --git a/example/lesson210-总结/Raster.h b/example/lesson210-总结/Raster.h new file mode 100644 index 0000000..52e025f --- /dev/null +++ b/example/lesson210-总结/Raster.h @@ -0,0 +1,246 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + _y = y; + } + else + { + _xStart = _xEnd; + _xEnd = _xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + Rgba _color1; + + int _x2; + int _y2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,int x2,int y2,Rgba color2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + void drawPoint(int x,int y, Rgba color,int ptSize); + + + + void drawArrays(DRAWMODE mode,const float2* points,int count); + + void drawFilleRect(int startX,int startY,int w,int h); + + void drawRect(const int2* points,const Rgba* colors); + + void drawImage(int startX,int startY,const Image* image); + + void drawImageWidthColorKey(int startX,int startY,const Image* image,Rgba key); + + void drawImageAlphaTest(int startX,int startY,const Image* image,byte alpha); + + void drawImageAlphaBlend(int startX,int startY,const Image* image,float alpha); + + + void drawImageAlpha( int startX,int startY,const Image* image,float alpha ); + + void drawImage(int startX,int startY,const Image* image,int x1,int y1,int w,int h); + + + void drawImageScale(int dstX,int dstY,int dstW,int dstH,const Image* image) + { + float xScale = (float)image->width()/(float)dstW; + float yScale = (float)image->height()/(float)dstH; + + for (int x = dstX ;x pixelAt(xx,yy); + setPixelEx(x,y,srcColor); + } + } + } + + public: + void drawTriangle(int2 p0,int2 p1,int2 p2,Rgba c0,Rgba c1,Rgba c2) + { + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, p1.x,p1.y,c1), + Ege(p1.x,p1.y,c1, p2.x,p2.y,c2), + Ege(p2.x,p2.y,c2, p0.x,p0.y,c0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + Span span(x1,x2,y,color1,color2); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson210-总结/lesson210-总结.vcproj b/example/lesson210-总结/lesson210-总结.vcproj new file mode 100644 index 0000000..1882cfa --- /dev/null +++ b/example/lesson210-总结/lesson210-总结.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson210-总结/lesson210.cpp b/example/lesson210-总结/lesson210.cpp new file mode 100644 index 0000000..d9f8312 --- /dev/null +++ b/example/lesson210-总结/lesson210.cpp @@ -0,0 +1,181 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + sprintf(szImage,"%s/image/grass.png",szPath); + CELL::Image* colorKey= CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + raster.drawTriangle(pt[0],pt[1],pt[2],color1,color2,color3); + + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + +// raster.drawImageAlphaTest(100,100,colorKey,100); +// raster.drawImageAlphaBlend(250,100,colorKey,0.3); +// +// //raster.drawImageAlpha(250,200,colorKey,0.5f); +// +// raster.drawImage(200,300,image1,50,50,30,50); + + raster.drawImageScale(100,100,250,100,image1); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + delete colorKey; + + return 0; +} \ No newline at end of file diff --git a/example/lesson300-纹理/CELLMath.hpp b/example/lesson300-纹理/CELLMath.hpp new file mode 100644 index 0000000..037619c --- /dev/null +++ b/example/lesson300-纹理/CELLMath.hpp @@ -0,0 +1,5950 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson300-纹理/CELLTimestamp.hpp b/example/lesson300-纹理/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson300-纹理/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson300-纹理/Image.hpp b/example/lesson300-纹理/Image.hpp new file mode 100644 index 0000000..eea3b73 --- /dev/null +++ b/example/lesson300-纹理/Image.hpp @@ -0,0 +1,51 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson300-纹理/Raster.cpp b/example/lesson300-纹理/Raster.cpp new file mode 100644 index 0000000..9f43c5f --- /dev/null +++ b/example/lesson300-纹理/Raster.cpp @@ -0,0 +1,63 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson300-纹理/Raster.h b/example/lesson300-纹理/Raster.h new file mode 100644 index 0000000..50468b4 --- /dev/null +++ b/example/lesson300-纹理/Raster.h @@ -0,0 +1,257 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + void drawTriangle(const Vertex& vertex) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv2), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1]); + drawEge(eges[iMax],eges[iShort2]); + + } + + void drawEge(const Ege& e1,const Ege& e2) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp( + span._colorStart + ,span._colorEnd + ,scale + ); + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson300-纹理/lesson300-纹理.vcproj b/example/lesson300-纹理/lesson300-纹理.vcproj new file mode 100644 index 0000000..692d1dc --- /dev/null +++ b/example/lesson300-纹理/lesson300-纹理.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson300-纹理/lesson300.cpp b/example/lesson300-纹理/lesson300.cpp new file mode 100644 index 0000000..9028f0d --- /dev/null +++ b/example/lesson300-纹理/lesson300.cpp @@ -0,0 +1,177 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/test.bmp",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + CELL::Raster::Vertex vertex = + { + CELL::int2(100,10), CELL::float2(0.5f,1.0f), CELL::Rgba(), + CELL::int2(200,100), CELL::float2(1.0f,0.0f), CELL::Rgba(), + CELL::int2(10,100), CELL::float2(0.0f,0.0f), CELL::Rgba(), + }; + + + raster.drawTriangle(vertex); + + TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson301-纹理/CELLMath.hpp b/example/lesson301-纹理/CELLMath.hpp new file mode 100644 index 0000000..037619c --- /dev/null +++ b/example/lesson301-纹理/CELLMath.hpp @@ -0,0 +1,5950 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + operator unsigned() + { + unsigned color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator int() + { + int color; + char* pColor = (char*)&color; + pColor[0] = _r; + pColor[1] = _g; + pColor[2] = _b; + pColor[3] = _a; + return color; + } + operator ulong() + { + return toUint(); + } + uint toUint() + { + return (_b) | (_g << 8) | (_r << 16) | (_a << 24); + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson301-纹理/CELLTimestamp.hpp b/example/lesson301-纹理/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson301-纹理/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson301-纹理/Image.hpp b/example/lesson301-纹理/Image.hpp new file mode 100644 index 0000000..7133f09 --- /dev/null +++ b/example/lesson301-纹理/Image.hpp @@ -0,0 +1,60 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + + return pixelAt(x,y); + + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson301-纹理/Raster.cpp b/example/lesson301-纹理/Raster.cpp new file mode 100644 index 0000000..9f43c5f --- /dev/null +++ b/example/lesson301-纹理/Raster.cpp @@ -0,0 +1,63 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson301-纹理/Raster.h b/example/lesson301-纹理/Raster.h new file mode 100644 index 0000000..f233247 --- /dev/null +++ b/example/lesson301-纹理/Raster.h @@ -0,0 +1,263 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { +// Rgba color = colorLerp( +// span._colorStart +// ,span._colorEnd +// ,scale +// ); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba color = image->pixelUV(uv.x,uv.y); + + scale += step; + + setPixel(x,span._y,color); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson301-纹理/lesson301-纹理.vcproj b/example/lesson301-纹理/lesson301-纹理.vcproj new file mode 100644 index 0000000..d8d7266 --- /dev/null +++ b/example/lesson301-纹理/lesson301-纹理.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson301-纹理/lesson301.cpp b/example/lesson301-纹理/lesson301.cpp new file mode 100644 index 0000000..12b53d7 --- /dev/null +++ b/example/lesson301-纹理/lesson301.cpp @@ -0,0 +1,185 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + CELL::Raster::Vertex vertex = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(10,110), CELL::float2(0.0f,1.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + }; + + CELL::Raster::Vertex vertex1 = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + CELL::int2(110,10), CELL::float2(1.0f,0.0f), CELL::Rgba(), + }; + + + raster.drawTriangle(vertex,image1); + raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson302-纹理-color-blend/CELLMath.hpp b/example/lesson302-纹理-color-blend/CELLMath.hpp new file mode 100644 index 0000000..b5219af --- /dev/null +++ b/example/lesson302-纹理-color-blend/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + + friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right) + { + return Rgba4Byte(left._r * right._r + ,left._g * right._g + ,left._b * right._b + ,left._a * right._a); + } + operator unsigned() + { + return _color; + } + uint toUint() + { + return _color; + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson302-纹理-color-blend/CELLTimestamp.hpp b/example/lesson302-纹理-color-blend/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson302-纹理-color-blend/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson302-纹理-color-blend/Image.hpp b/example/lesson302-纹理-color-blend/Image.hpp new file mode 100644 index 0000000..7133f09 --- /dev/null +++ b/example/lesson302-纹理-color-blend/Image.hpp @@ -0,0 +1,60 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + + return pixelAt(x,y); + + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson302-纹理-color-blend/Raster.cpp b/example/lesson302-纹理-color-blend/Raster.cpp new file mode 100644 index 0000000..9f43c5f --- /dev/null +++ b/example/lesson302-纹理-color-blend/Raster.cpp @@ -0,0 +1,63 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson302-纹理-color-blend/Raster.h b/example/lesson302-纹理-color-blend/Raster.h new file mode 100644 index 0000000..cc9df78 --- /dev/null +++ b/example/lesson302-纹理-color-blend/Raster.h @@ -0,0 +1,260 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp(span._colorStart,span._colorEnd,scale); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba pixel = image->pixelUV(uv.x,uv.y); + + Rgba dst = color + pixel; + scale += step; + + setPixel(x,span._y,dst); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson302-纹理-color-blend/lesson302-纹理-color-blend.vcproj b/example/lesson302-纹理-color-blend/lesson302-纹理-color-blend.vcproj new file mode 100644 index 0000000..6baf4cd --- /dev/null +++ b/example/lesson302-纹理-color-blend/lesson302-纹理-color-blend.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson302-纹理-color-blend/lesson302.cpp b/example/lesson302-纹理-color-blend/lesson302.cpp new file mode 100644 index 0000000..12b53d7 --- /dev/null +++ b/example/lesson302-纹理-color-blend/lesson302.cpp @@ -0,0 +1,185 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + CELL::Raster::Vertex vertex = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(10,110), CELL::float2(0.0f,1.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + }; + + CELL::Raster::Vertex vertex1 = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + CELL::int2(110,10), CELL::float2(1.0f,0.0f), CELL::Rgba(), + }; + + + raster.drawTriangle(vertex,image1); + raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson303-状态机/CELLMath.hpp b/example/lesson303-状态机/CELLMath.hpp new file mode 100644 index 0000000..b5219af --- /dev/null +++ b/example/lesson303-状态机/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + + friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right) + { + return Rgba4Byte(left._r * right._r + ,left._g * right._g + ,left._b * right._b + ,left._a * right._a); + } + operator unsigned() + { + return _color; + } + uint toUint() + { + return _color; + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson303-状态机/CELLTimestamp.hpp b/example/lesson303-状态机/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson303-状态机/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson303-状态机/Image.hpp b/example/lesson303-状态机/Image.hpp new file mode 100644 index 0000000..7133f09 --- /dev/null +++ b/example/lesson303-状态机/Image.hpp @@ -0,0 +1,60 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + + return pixelAt(x,y); + + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson303-状态机/Raster.cpp b/example/lesson303-状态机/Raster.cpp new file mode 100644 index 0000000..0b93db1 --- /dev/null +++ b/example/lesson303-状态机/Raster.cpp @@ -0,0 +1,66 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + memset(&_poitionPointer,0, sizeof(_poitionPointer)); + memset(&_colorPointer, 0, sizeof(_colorPointer)); + memset(&_uvPointer, 0, sizeof(_uvPointer)); + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson303-状态机/Raster.h b/example/lesson303-状态机/Raster.h new file mode 100644 index 0000000..a172cc5 --- /dev/null +++ b/example/lesson303-状态机/Raster.h @@ -0,0 +1,311 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + DM_TRIANGES = 4, + }; + + enum DATETYPE + { + DT_BYTE, + DT_FLOAT, + DT_DOUBLE, + }; + + struct DateElementDes + { + int _size; + DATETYPE _type; + int _stride; + const void* _data; + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + + DateElementDes _poitionPointer; + DateElementDes _colorPointer; + DateElementDes _uvPointer; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + + + void vertexPointer(int size,DATETYPE type,int stride,const void* data) + { + _poitionPointer._size = size; + _poitionPointer._type = type; + _poitionPointer._stride = stride; + _poitionPointer._data = data; + } + + void colorPointer(int size,DATETYPE type,int stride,const void* data) + { + _colorPointer._size = size; + _colorPointer._type = type; + _colorPointer._stride = stride; + _colorPointer._data = data; + } + + void textureCoordPointer(int size,DATETYPE type,int stride,const void* data) + { + _uvPointer._size = size; + _uvPointer._type = type; + _uvPointer._stride = stride; + _uvPointer._data = data; + } + + void drawArrays(DRAWMODE pri,int start,int count) + { + + } + + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp(span._colorStart,span._colorEnd,scale); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba pixel = image->pixelUV(uv.x,uv.y); + + Rgba dst = color + pixel; + scale += step; + + setPixel(x,span._y,dst); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson303-状态机/lesson303-状态机.vcproj b/example/lesson303-状态机/lesson303-状态机.vcproj new file mode 100644 index 0000000..7e78f16 --- /dev/null +++ b/example/lesson303-状态机/lesson303-状态机.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson303-状态机/lesson303.cpp b/example/lesson303-状态机/lesson303.cpp new file mode 100644 index 0000000..21d1189 --- /dev/null +++ b/example/lesson303-状态机/lesson303.cpp @@ -0,0 +1,204 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + struct Vertex + { + float x,y; + float u,v; + CELL::Rgba color; + }; + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + CELL::Raster::Vertex vertex = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(10,110), CELL::float2(0.0f,1.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + }; + + CELL::Raster::Vertex vertex1 = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + CELL::int2(110,10), CELL::float2(1.0f,0.0f), CELL::Rgba(), + }; + + Vertex vertexs[] = + { + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {110, 110, 1.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + {110, 10, 1.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + }; + + raster.vertexPointer(2,CELL::DT_FLOAT, sizeof(Vertex),&vertexs[0].x); + raster.textureCoordPointer(2,CELL::DT_FLOAT,sizeof(Vertex),&vertexs[0].u); + raster.colorPointer(4,CELL::DT_BYTE, sizeof(Vertex),&vertexs[0].color); + + raster.drawArrays(CELL::DM_TRIANGES,0,3); + + //raster.drawTriangle(vertex,image1); + //raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson304-状态机2/CELLMath.hpp b/example/lesson304-状态机2/CELLMath.hpp new file mode 100644 index 0000000..b5219af --- /dev/null +++ b/example/lesson304-状态机2/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + + friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right) + { + return Rgba4Byte(left._r * right._r + ,left._g * right._g + ,left._b * right._b + ,left._a * right._a); + } + operator unsigned() + { + return _color; + } + uint toUint() + { + return _color; + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson304-状态机2/CELLTimestamp.hpp b/example/lesson304-状态机2/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson304-状态机2/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson304-状态机2/Image.hpp b/example/lesson304-状态机2/Image.hpp new file mode 100644 index 0000000..7133f09 --- /dev/null +++ b/example/lesson304-状态机2/Image.hpp @@ -0,0 +1,60 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + + return pixelAt(x,y); + + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson304-状态机2/Raster.cpp b/example/lesson304-状态机2/Raster.cpp new file mode 100644 index 0000000..0b93db1 --- /dev/null +++ b/example/lesson304-状态机2/Raster.cpp @@ -0,0 +1,66 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _width = w; + _height = h; + _buffer = (uint*)buffer; + memset(&_poitionPointer,0, sizeof(_poitionPointer)); + memset(&_colorPointer, 0, sizeof(_colorPointer)); + memset(&_uvPointer, 0, sizeof(_uvPointer)); + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson304-状态机2/Raster.h b/example/lesson304-状态机2/Raster.h new file mode 100644 index 0000000..7aa742b --- /dev/null +++ b/example/lesson304-状态机2/Raster.h @@ -0,0 +1,376 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + DM_TRIANGES = 4, + }; + + enum DATETYPE + { + DT_BYTE, + DT_FLOAT, + DT_DOUBLE, + }; + + struct DateElementDes + { + int _size; + DATETYPE _type; + int _stride; + const void* _data; + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + + DateElementDes _poitionPointer; + DateElementDes _colorPointer; + DateElementDes _uvPointer; + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + + + void vertexPointer(int size,DATETYPE type,int stride,const void* data) + { + _poitionPointer._size = size; + _poitionPointer._type = type; + _poitionPointer._stride = stride; + _poitionPointer._data = data; + } + + void colorPointer(int size,DATETYPE type,int stride,const void* data) + { + _colorPointer._size = size; + _colorPointer._type = type; + _colorPointer._stride = stride; + _colorPointer._data = data; + } + + void textureCoordPointer(int size,DATETYPE type,int stride,const void* data) + { + _uvPointer._size = size; + _uvPointer._type = type; + _uvPointer._stride = stride; + _uvPointer._data = data; + } + + void drawArrays(DRAWMODE pri,int start,int count) + { + if (_poitionPointer._data == 0) + { + return; + } + char* posData = (char*)_poitionPointer._data; + char* cData = (char*)_colorPointer._data; + char* uvData = (char*)_uvPointer._data; + + float* fData = (float*)posData; + int2 p0 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p1 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p2 (fData[0],fData[1]); + + + + Rgba* pColor = (Rgba*)cData; + Rgba c0 (*pColor); + cData += _colorPointer._stride; + Rgba c1 (*pColor); + cData += _colorPointer._stride; + Rgba c2 (*pColor); + + float* pUV = (float*)uvData; + float2 uv0 (pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv1(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv2(pUV[0],pUV[1]); + + + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, uv0, p1.x,p1.y,c1, uv1), + Ege(p1.x,p1.y,c1, uv1, p2.x,p2.y,c2, uv2), + Ege(p2.x,p2.y,c2, uv2, p0.x,p0.y,c0, uv0), + }; + drawTrianle(eges,0); + } + + + void drawTrianle(Ege eges[3],Image* image) + { + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + } + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + Rgba color = colorLerp(span._colorStart,span._colorEnd,scale); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba pixel = image->pixelUV(uv.x,uv.y); + + Rgba dst = color + pixel; + scale += step; + + setPixel(x,span._y,dst); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson304-状态机2/lesson304-状态机2.vcproj b/example/lesson304-状态机2/lesson304-状态机2.vcproj new file mode 100644 index 0000000..d4c0ebf --- /dev/null +++ b/example/lesson304-状态机2/lesson304-状态机2.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson304-状态机2/lesson304.cpp b/example/lesson304-状态机2/lesson304.cpp new file mode 100644 index 0000000..21d1189 --- /dev/null +++ b/example/lesson304-状态机2/lesson304.cpp @@ -0,0 +1,204 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + struct Vertex + { + float x,y; + float u,v; + CELL::Rgba color; + }; + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + CELL::Raster::Vertex vertex = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(10,110), CELL::float2(0.0f,1.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + }; + + CELL::Raster::Vertex vertex1 = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + CELL::int2(110,10), CELL::float2(1.0f,0.0f), CELL::Rgba(), + }; + + Vertex vertexs[] = + { + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {110, 110, 1.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + {110, 10, 1.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + }; + + raster.vertexPointer(2,CELL::DT_FLOAT, sizeof(Vertex),&vertexs[0].x); + raster.textureCoordPointer(2,CELL::DT_FLOAT,sizeof(Vertex),&vertexs[0].u); + raster.colorPointer(4,CELL::DT_BYTE, sizeof(Vertex),&vertexs[0].color); + + raster.drawArrays(CELL::DM_TRIANGES,0,3); + + //raster.drawTriangle(vertex,image1); + //raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson305-状态机3/CELLMath.hpp b/example/lesson305-状态机3/CELLMath.hpp new file mode 100644 index 0000000..b5219af --- /dev/null +++ b/example/lesson305-状态机3/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + + friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right) + { + return Rgba4Byte(left._r * right._r + ,left._g * right._g + ,left._b * right._b + ,left._a * right._a); + } + operator unsigned() + { + return _color; + } + uint toUint() + { + return _color; + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson305-状态机3/CELLTimestamp.hpp b/example/lesson305-状态机3/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson305-状态机3/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson305-状态机3/Image.hpp b/example/lesson305-状态机3/Image.hpp new file mode 100644 index 0000000..7133f09 --- /dev/null +++ b/example/lesson305-状态机3/Image.hpp @@ -0,0 +1,60 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + + return pixelAt(x,y); + + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson305-状态机3/Raster.cpp b/example/lesson305-状态机3/Raster.cpp new file mode 100644 index 0000000..b7387b4 --- /dev/null +++ b/example/lesson305-状态机3/Raster.cpp @@ -0,0 +1,77 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _texture = 0; + _width = w; + _height = h; + _buffer = (uint*)buffer; + memset(&_poitionPointer,0, sizeof(_poitionPointer)); + memset(&_colorPointer, 0, sizeof(_colorPointer)); + memset(&_uvPointer, 0, sizeof(_uvPointer)); + + _defaultColorPointer._size = 4; + _defaultColorPointer._type = DT_BYTE; + _defaultColorPointer._stride= sizeof(Rgba); + _defaultColorPointer._data = _defaultColorArray; + + _defaultUVPointer._size = 2; + _defaultUVPointer._type = DT_FLOAT; + _defaultUVPointer._stride = sizeof(float2); + _defaultUVPointer._data = _detaultUVArray; + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson305-状态机3/Raster.h b/example/lesson305-状态机3/Raster.h new file mode 100644 index 0000000..cd5f942 --- /dev/null +++ b/example/lesson305-状态机3/Raster.h @@ -0,0 +1,418 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + DM_TRIANGES = 4, + }; + + enum DATETYPE + { + DT_BYTE, + DT_FLOAT, + DT_DOUBLE, + }; + + struct DateElementDes + { + int _size; + DATETYPE _type; + int _stride; + const void* _data; + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + + Image* _texture; + + DateElementDes _poitionPointer; + DateElementDes _colorPointer; + DateElementDes _uvPointer; + + DateElementDes _defaultColorPointer; + DateElementDes _defaultUVPointer; + Rgba _defaultColorArray[3]; + float2 _detaultUVArray[3]; + + + + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + + + void vertexPointer(int size,DATETYPE type,int stride,const void* data) + { + _poitionPointer._size = size; + _poitionPointer._type = type; + _poitionPointer._stride = stride; + _poitionPointer._data = data; + } + + void colorPointer(int size,DATETYPE type,int stride,const void* data) + { + _colorPointer._size = size; + _colorPointer._type = type; + _colorPointer._stride = stride; + _colorPointer._data = data; + } + + void textureCoordPointer(int size,DATETYPE type,int stride,const void* data) + { + _uvPointer._size = size; + _uvPointer._type = type; + _uvPointer._stride = stride; + _uvPointer._data = data; + } + + void bindTexture(Image* image) + { + _texture = image; + } + + void drawArrays(DRAWMODE pri,int start,int count) + { + if (_poitionPointer._data == 0) + { + return; + } + + DateElementDes colorPointerdesc = _colorPointer; + DateElementDes uvPointerdesc = _uvPointer; + if (colorPointerdesc._data == 0) + { + colorPointerdesc = _defaultColorPointer; + } + if (uvPointerdesc._data == 0) + { + uvPointerdesc = _defaultUVPointer; + } + char* posData = (char*)_poitionPointer._data; + char* cData = (char*)colorPointerdesc._data; + char* uvData = (char*)uvPointerdesc._data; + + for(int i = start ;i < start + count; i += 3) + { + float* fData = (float*)posData; + int2 p0 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p1 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p2 (fData[0],fData[1]); + posData += _poitionPointer._stride; + + + Rgba* pColor = (Rgba*)cData; + Rgba c0 (*pColor); + cData += _colorPointer._stride; + Rgba c1 (*pColor); + cData += _colorPointer._stride; + Rgba c2 (*pColor); + cData += _colorPointer._stride; + + float* pUV = (float*)uvData; + float2 uv0 (pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv1(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv2(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + + + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, uv0, p1.x,p1.y,c1, uv1), + Ege(p1.x,p1.y,c1, uv1, p2.x,p2.y,c2, uv2), + Ege(p2.x,p2.y,c2, uv2, p0.x,p0.y,c0, uv0), + }; + drawTrianle(eges); + + if (_colorPointer._data == 0) + { + cData = (char*)colorPointerdesc._data; + } + if (_uvPointer._data == 0 ) + { + + uvData = (char*)uvPointerdesc._data; + } + } + + } + + + void drawTrianle(Ege eges[3]) + { + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],_texture); + drawEge(eges[iMax],eges[iShort2],_texture); + } + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + //Rgba color = colorLerp(span._colorStart,span._colorEnd,scale); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba pixel = image->pixelUV(uv.x,uv.y); + + //Rgba dst = color + pixel; + scale += step; + + setPixel(x,span._y,pixel); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson305-状态机3/lesson305-状态机3.vcproj b/example/lesson305-状态机3/lesson305-状态机3.vcproj new file mode 100644 index 0000000..f17e6cc --- /dev/null +++ b/example/lesson305-状态机3/lesson305-状态机3.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson305-状态机3/lesson305.cpp b/example/lesson305-状态机3/lesson305.cpp new file mode 100644 index 0000000..3c6a629 --- /dev/null +++ b/example/lesson305-状态机3/lesson305.cpp @@ -0,0 +1,210 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + struct Vertex + { + float x,y; + float u,v; + CELL::Rgba color; + }; + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + CELL::Raster::Vertex vertex = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(10,110), CELL::float2(0.0f,1.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + }; + + CELL::Raster::Vertex vertex1 = + { + CELL::int2(10,10), CELL::float2(0.0f,0.0f), CELL::Rgba(), + CELL::int2(110,110), CELL::float2(1.0f,1.0f), CELL::Rgba(), + CELL::int2(110,10), CELL::float2(1.0f,0.0f), CELL::Rgba(), + }; + + Vertex vertexs[] = + { + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {110, 110, 1.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + {110, 10, 1.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {110, 110, 1.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + {10, 110, 0.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + }; + + raster.bindTexture(image1); + + raster.vertexPointer(2,CELL::DT_FLOAT, sizeof(Vertex),&vertexs[0].x); + raster.textureCoordPointer(2,CELL::DT_FLOAT,sizeof(Vertex),&vertexs[0].u); + //raster.colorPointer(4,CELL::DT_BYTE, sizeof(Vertex),&vertexs[0].color); + + raster.drawArrays(CELL::DM_TRIANGES,0,6); + + //raster.drawTriangle(vertex,image1); + //raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson306-纹理包装-重复/CELLMath.hpp b/example/lesson306-纹理包装-重复/CELLMath.hpp new file mode 100644 index 0000000..b5219af --- /dev/null +++ b/example/lesson306-纹理包装-重复/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + + friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right) + { + return Rgba4Byte(left._r * right._r + ,left._g * right._g + ,left._b * right._b + ,left._a * right._a); + } + operator unsigned() + { + return _color; + } + uint toUint() + { + return _color; + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson306-纹理包装-重复/CELLTimestamp.hpp b/example/lesson306-纹理包装-重复/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson306-纹理包装-重复/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson306-纹理包装-重复/Image.hpp b/example/lesson306-纹理包装-重复/Image.hpp new file mode 100644 index 0000000..4bb333e --- /dev/null +++ b/example/lesson306-纹理包装-重复/Image.hpp @@ -0,0 +1,60 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + public: + Image(int w,int h,void* data) + { + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + + return pixelAt((unsigned)(x)%_width,(unsigned)(y)%_height); + + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson306-纹理包装-重复/Raster.cpp b/example/lesson306-纹理包装-重复/Raster.cpp new file mode 100644 index 0000000..b7387b4 --- /dev/null +++ b/example/lesson306-纹理包装-重复/Raster.cpp @@ -0,0 +1,77 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _texture = 0; + _width = w; + _height = h; + _buffer = (uint*)buffer; + memset(&_poitionPointer,0, sizeof(_poitionPointer)); + memset(&_colorPointer, 0, sizeof(_colorPointer)); + memset(&_uvPointer, 0, sizeof(_uvPointer)); + + _defaultColorPointer._size = 4; + _defaultColorPointer._type = DT_BYTE; + _defaultColorPointer._stride= sizeof(Rgba); + _defaultColorPointer._data = _defaultColorArray; + + _defaultUVPointer._size = 2; + _defaultUVPointer._type = DT_FLOAT; + _defaultUVPointer._stride = sizeof(float2); + _defaultUVPointer._data = _detaultUVArray; + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson306-纹理包装-重复/Raster.h b/example/lesson306-纹理包装-重复/Raster.h new file mode 100644 index 0000000..cd5f942 --- /dev/null +++ b/example/lesson306-纹理包装-重复/Raster.h @@ -0,0 +1,418 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + DM_TRIANGES = 4, + }; + + enum DATETYPE + { + DT_BYTE, + DT_FLOAT, + DT_DOUBLE, + }; + + struct DateElementDes + { + int _size; + DATETYPE _type; + int _stride; + const void* _data; + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + + Image* _texture; + + DateElementDes _poitionPointer; + DateElementDes _colorPointer; + DateElementDes _uvPointer; + + DateElementDes _defaultColorPointer; + DateElementDes _defaultUVPointer; + Rgba _defaultColorArray[3]; + float2 _detaultUVArray[3]; + + + + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + + + void vertexPointer(int size,DATETYPE type,int stride,const void* data) + { + _poitionPointer._size = size; + _poitionPointer._type = type; + _poitionPointer._stride = stride; + _poitionPointer._data = data; + } + + void colorPointer(int size,DATETYPE type,int stride,const void* data) + { + _colorPointer._size = size; + _colorPointer._type = type; + _colorPointer._stride = stride; + _colorPointer._data = data; + } + + void textureCoordPointer(int size,DATETYPE type,int stride,const void* data) + { + _uvPointer._size = size; + _uvPointer._type = type; + _uvPointer._stride = stride; + _uvPointer._data = data; + } + + void bindTexture(Image* image) + { + _texture = image; + } + + void drawArrays(DRAWMODE pri,int start,int count) + { + if (_poitionPointer._data == 0) + { + return; + } + + DateElementDes colorPointerdesc = _colorPointer; + DateElementDes uvPointerdesc = _uvPointer; + if (colorPointerdesc._data == 0) + { + colorPointerdesc = _defaultColorPointer; + } + if (uvPointerdesc._data == 0) + { + uvPointerdesc = _defaultUVPointer; + } + char* posData = (char*)_poitionPointer._data; + char* cData = (char*)colorPointerdesc._data; + char* uvData = (char*)uvPointerdesc._data; + + for(int i = start ;i < start + count; i += 3) + { + float* fData = (float*)posData; + int2 p0 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p1 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p2 (fData[0],fData[1]); + posData += _poitionPointer._stride; + + + Rgba* pColor = (Rgba*)cData; + Rgba c0 (*pColor); + cData += _colorPointer._stride; + Rgba c1 (*pColor); + cData += _colorPointer._stride; + Rgba c2 (*pColor); + cData += _colorPointer._stride; + + float* pUV = (float*)uvData; + float2 uv0 (pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv1(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv2(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + + + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, uv0, p1.x,p1.y,c1, uv1), + Ege(p1.x,p1.y,c1, uv1, p2.x,p2.y,c2, uv2), + Ege(p2.x,p2.y,c2, uv2, p0.x,p0.y,c0, uv0), + }; + drawTrianle(eges); + + if (_colorPointer._data == 0) + { + cData = (char*)colorPointerdesc._data; + } + if (_uvPointer._data == 0 ) + { + + uvData = (char*)uvPointerdesc._data; + } + } + + } + + + void drawTrianle(Ege eges[3]) + { + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],_texture); + drawEge(eges[iMax],eges[iShort2],_texture); + } + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + //Rgba color = colorLerp(span._colorStart,span._colorEnd,scale); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba pixel = image->pixelUV(uv.x,uv.y); + + //Rgba dst = color + pixel; + scale += step; + + setPixel(x,span._y,pixel); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson306-纹理包装-重复/lesson306-纹理包装-重复.vcproj b/example/lesson306-纹理包装-重复/lesson306-纹理包装-重复.vcproj new file mode 100644 index 0000000..d8d7af2 --- /dev/null +++ b/example/lesson306-纹理包装-重复/lesson306-纹理包装-重复.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson306-纹理包装-重复/lesson306.cpp b/example/lesson306-纹理包装-重复/lesson306.cpp new file mode 100644 index 0000000..8f3b915 --- /dev/null +++ b/example/lesson306-纹理包装-重复/lesson306.cpp @@ -0,0 +1,206 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + struct Vertex + { + float x,y; + float u,v; + CELL::Rgba color; + }; + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + + static float step = 0; + + Vertex vertexs[] = + { + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {210, 210, 1.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + {210, 10, 1.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {210, 210, 1.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + {10, 210, 0.0f, 1.0f, CELL::Rgba(255,255,255,255)}, + }; + + for (int i = 0 ;i < 6 ; ++ i ) + { + vertexs[i].v += step; + } + + step += 0.1f; + + raster.bindTexture(image1); + + raster.vertexPointer(2,CELL::DT_FLOAT, sizeof(Vertex),&vertexs[0].x); + raster.textureCoordPointer(2,CELL::DT_FLOAT,sizeof(Vertex),&vertexs[0].u); + //raster.colorPointer(4,CELL::DT_BYTE, sizeof(Vertex),&vertexs[0].color); + + raster.drawArrays(CELL::DM_TRIANGES,0,6); + + //raster.drawTriangle(vertex,image1); + //raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson307-纹理包装-clamage/CELLMath.hpp b/example/lesson307-纹理包装-clamage/CELLMath.hpp new file mode 100644 index 0000000..b5219af --- /dev/null +++ b/example/lesson307-纹理包装-clamage/CELLMath.hpp @@ -0,0 +1,5938 @@ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace CELL +{ + + #define PI 3.14159265358979323 + #define TWO_PI 6.28318530717958647 + #define HALF_PI 1.57079632679489661 + #define DEG2RAD(theta) (0.01745329251994329 * (theta)) + #define RAD2DEG 57.2957795130823208 + #define LOG2 0.69314718055994529 + #define WGS_84_RADIUS_EQUATOR 6378137.0 + #define WGS_84_RADIUS_POLAR 6356752.3142 + #define MIN(a,b) ((a) < (b) ? (a) : (b)) + #define MAX(a,b) ((a) > (b) ? (a) : (b)) + #ifndef FLT_MAX + #define FLT_MAX 3.402823466e+38F + #endif + + #ifndef FLT_MIN + #define FLT_MIN 1.175494351e-38F + #endif + + #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) + + typedef unsigned char byte; + typedef long long int64; + typedef unsigned short ushort; + typedef unsigned int uint; + typedef unsigned long ulong; + + template inline T tmin(T a,T b) + { + return a < b ? a:b; + } + + template inline T tmax(T a,T b) + { + return a > b ? a:b; + } + + + union LargeInt + { + struct __LARGE_INT + { + unsigned int LowPart; + unsigned int HighPart; + }_largeInt; + int64 int64Data; + } ; + + + inline float unitRandom () + { + return float(rand()) / float( RAND_MAX ); + } + + //----------------------------------------------------------------------- + inline float rangeRandom (float fLow, float fHigh) + { + return (fHigh-fLow)*unitRandom() + fLow; + } + + /** + * 64λ + */ + inline int64 makeInt64(unsigned low,unsigned hi) + { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; + } + + + template + struct tvec2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[]( size_type i ) const + { + assert(i < this->length()); + return (&x)[i]; + } + tvec2() : + x(value_type(0)), + y(value_type(0)) + {} + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) + {} + tvec2(value_type const & s) : + x(s), + y(s) + {} + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) + {} + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) + {} + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) + {} + + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) + {} + + tvec2 & operator= (tvec2 const & v) + { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator= (tvec2 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + + template + tvec2 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) + { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) + { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() + { + ++ this->x; + ++ this->y; + return *this; + } + tvec2 & operator--() + { + --this->x; + --this->y; + return *this; + } + void makeCeil( tvec2 cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + } + void makeFloor( tvec2 cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + } + }; + + template + tvec2 rotate(tvec2 const & v, T angle) + { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + + template + bool operator==(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y); + } + + template + bool operator!=(tvec2 const & v1, tvec2 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y); + } + + template + tvec2 operator+ (tvec2 const & v, T const & s) + { + return tvec2( + v.x + T(s), + v.y + T(s)); + } + + template + tvec2 operator+ (T const & s, tvec2 const & v) + { + return tvec2( + T(s) + v.x, + T(s) + v.y); + } + + template + tvec2 operator+ (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); + } + + template + tvec2 operator-(tvec2 const & v, T const & s) + { + return tvec2( + v.x - T(s), + v.y - T(s)); + } + + template + tvec2 operator- (T const & s, tvec2 const & v) + { + return tvec2( + T(s) - v.x, + T(s) - v.y); + } + + template + tvec2 operator- (tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); + } + + template + tvec2 operator* (tvec2 const & v, T const & s) + { + return tvec2( + v.x * T(s), + v.y * T(s)); + } + + template + tvec2 operator* (T const & s, tvec2 const & v) + { + return tvec2( + T(s) * v.x, + T(s) * v.y); + } + + template + tvec2 operator*(tvec2 const & v1, tvec2 const & v2) + { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); + } + + template + tvec2 operator/(tvec2 const & v, T const & s) + { + return tvec2( + v.x / T(s), + v.y / T(s)); + } + + template + tvec2 operator/(T const & s, tvec2 const & v) + { + return tvec2( + T(s) / v.x, + T(s) / v.y); + } + + template + tvec2 operator/ (tvec2 const & v1, tvec2 const & v2 ) + { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); + } + + template + tvec2 operator- (tvec2 const & v) + { + return tvec2 ( + -v.x, + -v.y + ); + } + + template + tvec2 operator++ (tvec2 const & v, int) + { + return tvec2( + v.x + T(1), + v.y + T(1) + ); + } + + template + tvec2 operator-- (tvec2 const & v, int) + { + return tvec2( + v.x - T(1), + v.y - T(1) + ); + } + + + template + struct tvec3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const + { + return 3; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) + {} + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) + {} + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) + {} + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) + {} + + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) + {} + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) + {} + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) + {} + template + tvec3(A s,tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) + {} + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) + {} + + + tvec3& operator= (tvec3 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator= (tvec3 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + template + tvec3 & operator*=(U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + tvec3 & operator--() + { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor( const tvec3& cmp ) + { + if( cmp.x < x ) x = cmp.x; + if( cmp.y < y ) y = cmp.y; + if( cmp.z < z ) z = cmp.z; + } + void makeCeil( const tvec3& cmp ) + { + if( cmp.x > x ) x = cmp.x; + if( cmp.y > y ) y = cmp.y; + if( cmp.z > z ) z = cmp.z; + } + T lengthf() const + { + return (T)sqrtf( x * x + y * y + z * z ); + } + + }; + + template + bool operator >(const tvec3& left ,const tvec3& right) + { + return left.x > right.x && left.y > right.y && left.z > right.z; + } + + template + bool operator <(const tvec3& left ,const tvec3& right) + { + return left.x < right.x && left.y < right.y && left.z < right.z; + } + + template + tvec3 rotateX(const tvec3& v, T angle) + { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec3 rotateY(tvec3 const & v, T angle) + { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec3 rotateZ(tvec3 const & v, T angle) + { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + /** + * ļн + * A,B + * AB = |A|*|B|*cos(@) + * cos(@) = AB/|A|*|B| + * @ = acos(@) + */ + template + T angleBetweenVector(const tvec3& a, const tvec3& b) + { + #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + inline bool _isnan(T t) + { + return t == t; + } + + template + T angleBetweenVector(const tvec2& a, const tvec2& b) + { + #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b) ; + T angle = acos( dotProduct / vectorsMagnitude ); + T result = angle * T(RAD2DEG); + if(_isnan(result)) + { + return T(0); + } + else + { + return result; + } + } + + template + static T clamp(T val, T minval, T maxval) + { + assert (minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); + } + + template + inline T acosEx (T val) + { + if ( T(-1.0f) < val ) + { + if ( val < 1.0f ) + return T(acos(val)); + else + return T(0); + } + else + { + return T(PI); + } + } + + template + inline T angleBetween(const tvec3& a, const tvec3& b) + { + T lenProduct = a.lengthf() * b.lengthf(); + + // Divide by zero check + if(lenProduct < 1e-6f) + lenProduct = 1e-6f; + + float f = dot(a,b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + + } + + /** + * ڶ + * ڶУ򣬵ߵļн֮ == 360 + */ + template + bool insidePolyon( const tvec3& point, const tvec3 polygon[], size_t count) + { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool insidePolyon( const tvec2& point, const tvec2 polygon[], size_t count) + { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) + { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x,vA.y,0); + tvec3 b(vB.x,vB.y,0); + angle += angleBetweenVector(a, b); + } + if( abs(angle - 360 ) >= 0.5f) + { + return true; + } + return false; + } + + template + bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) + { + tvec3 v0 = C - A ; + tvec3 v1 = B - A ; + tvec3 v2 = P - A ; + + float dot00 = dot(v0,v0) ; + float dot01 = dot(v0,v1) ; + float dot02 = dot(v0,v2) ; + float dot11 = dot(v1,v1) ; + float dot12 = dot(v1,v2) ; + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ; + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false ; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false ; + } + + return u + v <= 1 ; + } + + + + template + bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) + { + return pointinTriangle( + tvec3(A.x,A.y,0), + tvec3(B.x,B.y,0), + tvec3(C.x,C.y,0), + tvec3(P.x,P.y,0)); + } + + + /** + * ཻ + */ + template + bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) + { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2 ); + + // If determinant is near zero, ray lies in plane of triangle + T det = dot( edge1,pvec ); + + tvec3 tvec; + if( det > 0 ) + { + tvec = orig - v0; + } + else + { + tvec = v0 - orig; + det = -det; + } + if( det < 0.0001f ) + return false; + // Calculate U parameter and test bounds + *u = dot( tvec, pvec ); + if( *u < 0.0f || *u > det ) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1 ); + + // Calculate V parameter and test bounds + *v = dot( dir, qvec ); + if( *v < T(0.0f) || *u + *v > det ) + return false; + + *t = dot( edge2,qvec ); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; + } + /** + * + */ + template T calcTriangleArea(const tvec3& pt1,const tvec3& pt2,const tvec3& pt3) + { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1,e2); + return length(e3) * T(0.5); + } + + + template + bool operator==(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); + } + + template + bool operator!=(tvec3 const & v1, tvec3 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); + } + + + template + tvec3 operator+(tvec3 const & v, T const & s) + { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); + } + + template + tvec3 operator+ ( T const & s, tvec3 const & v) + { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); + } + + template + tvec3 operator+ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v, T const & s) + { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); + } + + template + tvec3 operator- (T const & s, tvec3 const & v) + { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); + } + + template + tvec3 operator- (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); + } + + template + tvec3 operator*(tvec3 const & v, T const & s) + { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); + } + + template + tvec3 operator* (T const & s, tvec3 const & v) + { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); + } + + template + tvec3 operator* (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); + } + + template + tvec3 operator/ (tvec3 const & v, T const & s) + { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); + } + + template + tvec3 operator/ (T const & s, tvec3 const & v) + { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); + } + + template + tvec3 operator/ (tvec3 const & v1, tvec3 const & v2) + { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); + } + + template + tvec3 operator- (tvec3 const & v) + { + return tvec3( + -v.x, + -v.y, + -v.z); + } + + template + tvec3 operator++ (tvec3 const & v, int) + { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); + } + + template + tvec3 operator-- (tvec3 const & v, int) + { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); + } + template + struct tvec4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const + { + return 4; + } + + value_type & operator[](size_type i) + { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const + { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) + {} + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) + {} + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) + {} + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) + {} + + template + tvec4(tvec3 const & v, B s): + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) + {} + + template + tvec4(A s,tvec3 const & v): + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) + {} + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) + {} + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) + {} + + tvec4 & operator=(tvec4 const & v) + { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator= (tvec4 const & v) + { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) + { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + template + tvec4 & operator+=(tvec4 const & v) + { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) + { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) + { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + template + tvec4 & operator*= (U const & s) + { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=( tvec4 const & v) + { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + template + tvec4 & operator/=(U const & s) + { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) + { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() + { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() + { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } + }; + + template + tvec4 rotateX(const tvec4& v, T angle) + { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; + } + + template + tvec4 rotateY(tvec4 const & v, T angle) + { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; + } + + template + tvec4 rotateZ(tvec4 const & v, T angle) + { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; + } + template + tvec4 operator+ (tvec4 const & v, T const & s) + { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); + } + + template + tvec4 operator+ (T const & s, tvec4 const & v) + { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); + } + + template + tvec4 operator+ (tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); + } + + template + tvec4 operator- (tvec4 const & v, T const & s) + { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); + } + + template + tvec4 operator- (T const & s, tvec4 const & v) + { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); + } + + template + tvec4 operator- + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); + } + + template + tvec4 operator* (tvec4 const & v, T const & s) + { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); + } + + template + tvec4 operator* (T const & s, tvec4 const & v) + { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); + } + + template + tvec4 operator*(tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); + } + + template + tvec4 operator/ (tvec4 const & v, T const & s) + { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); + } + + template + tvec4 operator/ (T const & s, tvec4 const & v) + { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); + } + + template + tvec4 operator/ ( tvec4 const & v1, tvec4 const & v2) + { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); + } + + template + tvec4 operator- ( tvec4 const & v) + { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); + } + + + template + bool operator== + ( + tvec4 const & v1, + tvec4 const & v2 + ) + { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); + } + + template + bool operator!=(tvec4 const & v1, tvec4 const & v2) + { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); + } + + template + class trect + { + public: + trect(T left = 0,T top = 0,T right = 0,T bottom = 0) + { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + void fromCenter(T x,T y,T size) + { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x,T y,T sizeX,T sizeY) + { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x,T y) + { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const + { + return tvec2((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const + { + return tvec2((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f)); + } + public: + T _left; + T _top; + T _right; + T _bottom; + }; + + template + struct tmat2x2 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + + public: + tmat2x2 _inverse() const + { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + + this->value[1][1] / Determinant, + - this->value[0][1] / Determinant, + - this->value[1][0] / Determinant, + + this->value[0][0] / Determinant); + return Inverse; + } + + private: + col_type value[2]; + + public: + + size_type length() const + { + return 2; + } + + static size_type col_size() + { + return 2; + } + + static size_type row_size() + { + return 2; + } + + col_type &operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + col_type const &operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() + { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + tmat2x2(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) + { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) + { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) + { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) + { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + tmat2x2& operator=(tmat2x2 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) + { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*= ( U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*= (tmat2x2 const & m) + { + return (*this = *this * m); + } + + template + tmat2x2& operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/= (tmat2x2 const & m) + { + return (*this = *this / m); + } + + tmat2x2& operator++ () + { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator-- () + { + --this->value[0]; + --this->value[1]; + return *this; + }; + }; + + template + tmat2x2 rotate(T angle) + { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c,s,-s,c); + } + + template + tmat2x2 operator+ (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] + s,m[1] + s); + } + + template + tmat2x2 operator+ (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] + m2[0],m1[1] + m2[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] - s,m[1] - s); + } + + template + tmat2x2 operator- (T const & s, tmat2x2 const & m) + { + return tmat2x2(s - m[0],s - m[1]); + } + + template + tmat2x2 operator- (tmat2x2 const & m1, tmat2x2 const & m2) + { + return tmat2x2(m1[0] - m2[0],m1[1] - m2[1]); + } + + template + tmat2x2 operator* (tmat2x2 const & m, T const & s) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tmat2x2 operator* ( T const & s, tmat2x2 const & m) + { + return tmat2x2(m[0] * s,m[1] * s); + } + + template + tvec2 operator*(tmat2x2 const & m, tvec2 const & v) + { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); + } + + template + tvec2 operator*(tvec2 const & v, tmat2x2 const & m) + { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); + } + + template + tmat2x2 operator*(tmat2x2 const & m1,tmat2x2 const & m2) + { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); + } + + + + template + struct tmat3x3 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + + private: + // Data + col_type value[3]; + + public: + size_type length() const + { + return 3; + } + size_type col_size() + { + return 3; + } + + size_type row_size() + { + return 3; + } + + tmat3x3() + { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + + tmat3x3(value_type const & s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) + { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + + + tmat3x3 _inverse() const + { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + template + tmat3x3 & operator=(tmat3x3 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-= (tmat3x3 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + template + tmat3x3 & operator*= (tmat3x3 const & m) + { + return (*this = *this * m); + } + + template + tmat3x3 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/= (tmat3x3 const & m) + { + return (*this = *this / m); + } + + tmat3x3 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + tmat3x3 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + }; + + template + tmat3x3 operator+ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); + } + + template + tmat3x3 operator+ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); + } + + template + tmat3x3 operator- (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); + } + + template + tmat3x3 operator- (tmat3x3 const & m1, tmat3x3 const & m2) + { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); + } + + template + tmat3x3 operator* (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tmat3x3 operator* (T const & s, tmat3x3 const & m) + { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); + } + + template + tvec3 operator* (tmat3x3 const & m, tvec3 const & v) + { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); + } + + template + tvec3 operator* (tvec3 const & v, tmat3x3 const & m) + { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); + } + + template + tmat3x3 operator* (tmat3x3 const & m1, tmat3x3 const & m2) + { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; + } + + template + tmat3x3 operator/ (tmat3x3 const & m, T const & s) + { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); + } + + template + tmat3x3 operator/ (T const & s, tmat3x3 const & m) + { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); + } + + template + tvec3 operator/ (tmat3x3 const & m, tvec3 const & v) + { + return m._inverse() * v; + } + + template + tvec3 operator/ (tvec3 const & v, tmat3x3 const & m) + { + return v * m._inverse(); + } + + template + tmat3x3 operator/ (tmat3x3 const & m1, tmat3x3 const & m2) + { + return m1 * m2._inverse(); + } + + template + tmat3x3 const operator- (tmat3x3 const & m) + { + return tmat3x3( + -m[0], + -m[1], + -m[2]); + } + + template + tmat3x3 const operator++ (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); + } + + template + tmat3x3 const operator-- (tmat3x3 const & m, int) + { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); + } + template + bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); + } + + template + bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); + } + + + + template + struct tmat4x4 + { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + + public: + tmat4x4 inverse() const + { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + + this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + + private: + col_type value[4]; + public: + + size_type length() const + { + return 4; + } + size_type col_size() + { + return 4; + } + size_type row_size() + { + return 4; + } + void identify() + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + col_type & operator[](size_type i) + { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const + { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) + { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) + { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + tmat4x4() + { + } + + tmat4x4(value_type s) + { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) + { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) + { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) + { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) + { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) + { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) + { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const + { + return &this->value[0][0]; + } + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator= (tmat4x4 const & m) + { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+= (U const & s) + { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+= (tmat4x4 const & m) + { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-= (U const & s) + { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-= (tmat4x4 const & m) + { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*= (U const & s) + { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*= (tmat4x4 const & m) + { + return (*this = *this * m); + } + + template + tmat4x4 & operator/= (U const & s) + { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + template + tmat4x4 & operator/= (tmat4x4 const & m) + { + return (*this = *this / m); + } + + tmat4x4 & operator++ () + { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator-- () + { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + tmat4x4& translate( value_type x,value_type y,value_type z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + template + tmat4x4& translate( U x,U y,U z) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x,pos.y, pos.z, 1); + return *this; + } + template + tmat4x4& translate(tvec3 const& pos) + { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle,tvec3 const & v ) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + template + tmat4x4& rotateX(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0,-s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + template + tmat4x4& rotateY(U angle) + { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0,-s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + tmat4x4& rotateZ(value_type angle) + { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + template + tmat4x4& rotateZ(U angle) + { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type( c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type( 0, 0, 1, 0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0); + this->value[1] = col_type( 0, cosX, sinX, 0); + this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type( cosY, 0, sinY, 0); + this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0); + this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0); + this->value[3] = col_type( 0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll( T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) + { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x,value_type y,value_type z) + { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,U y,U z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x,V y,W z) + { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose( ) const + { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const + { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } + }; + + + + template + tmat4x4 rotateX(T angleX) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0),-sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateY(T angleY) + { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateZ(T angleZ) + { + T cosZ = (T)cos(DEG2RAD(angleZ)); + T sinZ = (T)sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXY(T angleX, T angleY) + { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateYX(T angleY, T angleX) + { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); + } + + template + tmat4x4 rotateXZ(T angleX, T angleZ) + { + return rotateX(angleX) * rotateZ(angleZ); + } + + template + tmat4x4 rotateZX(T angleX, T angleZ) + { + return rotateZ(angleZ) * rotateX(angleX); + } + + template + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) + { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; + } + + template + void axisAngle + ( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) + { + T epsilon = (T)0.01; + T epsilon2 = (T)0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) + { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) + { + angle = (T)0.0; + axis.x = (T)1.0; + axis.y = (T)0.0; + axis.z = (T)0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T)1.0) / (T)2.0; + T yy = (mat[1][1] + (T)1.0) / (T)2.0; + T zz = (mat[2][2] + (T)1.0) / (T)2.0; + T xy = (mat[1][0] + mat[0][1]) / (T)4.0; + T xz = (mat[2][0] + mat[0][2]) / (T)4.0; + T yz = (mat[2][1] + mat[1][2]) / (T)4.0; + if ((xx > yy) && (xx > zz)) + { + if (xx < epsilon) + { + axis.x = (T)0.0; + axis.y = (T)0.7071; + axis.z = (T)0.7071; + } + else + { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } + else if (yy > zz) + { + if (yy < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.0; + axis.z = (T)0.7071; + } + else + { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } + else + { + if (zz < epsilon) + { + axis.x = (T)0.7071; + axis.y = (T)0.7071; + axis.z = (T)0.0; + } + else + { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T)1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; + } + + template + tmat4x4 axisAngleMatrix(tvec3 const & axis,T const angle) + { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); + } + + template + tmat4x4 interpolate + ( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) + { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; + } + + template + tvec3 operator * (tvec3 const& v, tmat4x4 const& mat) + { + return tvec3 + ( + v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0], + v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1], + v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2] + ); + } + + template + tmat4x4 operator+ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); + } + + template + tmat4x4 operator+ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); + } + + template + tmat4x4 operator- (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); + } + + template + tmat4x4 operator- (tmat4x4 const & m1, tmat4x4 const & m2) + { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); + } + + template + tmat4x4 operator* (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + tmat4x4 operator* (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); + } + + template + typename tmat4x4::col_type operator* (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); + } + + template + typename tmat4x4::row_type operator* (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); + } + + + + + template + tmat4x4 operator* (tmat4x4 const & m1, tmat4x4 const & m2) + { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; + } + + template + tmat4x4 operator/ (tmat4x4 const & m, typename tmat4x4::value_type s) + { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); + } + + template + tmat4x4 operator/ (typename tmat4x4::value_type s, tmat4x4 const & m) + { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); + } + + template + typename tmat4x4::col_type operator/ (tmat4x4 const & m, typename tmat4x4::row_type const & v) + { + return m.inverse() * v; + } + + template + typename tmat4x4::row_type operator/ (typename tmat4x4::col_type const & v, tmat4x4 const & m) + { + return v * m.inverse(); + } + + template + tmat4x4 operator/ (tmat4x4 const & m1, tmat4x4 const & m2) + { + return m1 * m2.inverse(); + } + + template + tmat4x4 const operator- (tmat4x4 const & m) + { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); + } + + template + tmat4x4 const operator++ (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); + } + + template + tmat4x4 const operator-- (tmat4x4 const & m, int) + { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); + } + + template + bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); + } + + template + bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) + { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); + } + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + //! + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + typename tvec2::value_type length(tvec2 const & v) + { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); + } + + template + typename tvec3::value_type length(tvec3 const & v) + { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); + } + + template + typename tvec4::value_type length(tvec4 const & v) + { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); + } + template + typename tvec2::value_type distance(tvec2 const & p0,tvec2 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec3::value_type distance(tvec3 const & p0,tvec3 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec4::value_type distance(tvec4 const & p0,tvec4 const & p1) + { + return length(p1 - p0); + } + + template + typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) + { + return x.x * y.x + x.y * y.y; + } + + template + typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z; + } + template + typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) + { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; + } + + template + tvec3 cross(tvec3 const & x, tvec3 const & y) + { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); + } + + template + T inversesqrt(T x) + { + return T(1) / sqrt(x); + } + + template + tvec2 normalize(tvec2 const & x) + { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); + } + + template + tvec3 normalize(tvec3 const & x) + { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); + } + + template + tvec4 normalize(tvec4 const & x) + { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); + } + + + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////// + + template + struct tquat + { + typedef T value_type; + typedef std::size_t size_type; + + public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const + { + return 4; + } + + tquat(): + x(0), + y(0), + z(0), + w(1) + {} + explicit tquat(value_type s, tvec3 const & v): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(tvec3 const & v,value_type s): + x(v.x), + y(v.y), + z(v.z), + w(s) + { + } + explicit tquat(value_type w, value_type x, value_type y, value_type z): + x(x), + y(y), + z(z), + w(w) + {} + + explicit tquat(tvec3 const & eulerAngle) + { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) + { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) + { + *this = quat_cast(m); + } + + value_type & operator[](int i) + { + return (&x)[i]; + } + + value_type const & operator[](int i) const + { + return (&x)[i]; + } + + tquat & operator*=(value_type s) + { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator = (const tquat& right) + { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) + { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } + }; + + + template< typename T> + tmat4x4 makeTransform( tvec3 const & position, tvec3 const& scale, const tquat& orientation) + { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); + } + + template + T dot(tquat const & q1, tquat const & q2) + { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + + template + tquat cross(tquat const & q1, tquat const & q2) + { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); + } + + template + T length(tquat const & q) + { + return sqrt(dot(q, q)); + } + + template + genType mix(genType x, genType y, genType a) + { + return x + a * (y - x); + } + + template + T epsilon() + { + return std::numeric_limits::epsilon(); + } + + + + template + tquat conjugate(tquat const & q) + { + return tquat(q.w, -q.x, -q.y, -q.z); + } + + template + tquat inverse(tquat const & q) + { + return conjugate(q) / dot(q, q); + } + + template + bool operator==(tquat const & q1, tquat const & q2) + { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); + } + + template + bool operator!=(tquat const & q1, tquat const & q2) + { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); + } + + template + tquat operator- (tquat const & q) + { + return tquat(-q.w, -q.x, -q.y, -q.z); + } + + template + tquat operator+ ( tquat const & q, tquat const & p) + { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); + } + + template + tquat operator* ( tquat const & q, tquat const & p) + { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); + } + + template + tvec3 operator* (tquat const & q, tvec3 const & v) + { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; + } + + template + tvec3 operator* (tvec3 const & v,tquat const & q) + { + return inverse(q) * v; + } + + + template + tquat operator* (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); + } + + template + tquat operator* (typename tquat::value_type s,tquat const & q) + { + return q * s; + } + + template + tquat operator/ (tquat const & q, typename tquat::value_type s) + { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); + } + + + + template + tquat mix(tquat const & x, tquat const & y, T const & a) + { + T cosTheta = dot(x, y); + if(cosTheta > T(1) - epsilon()) + { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } + } + + template + tquat lerp(tquat const & x, tquat const & y, T a) + { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); + } + + template + tquat slerp(tquat const & x, tquat const & y, T a) + { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) + { + z = -y; + cosTheta = -cosTheta; + } + if(cosTheta > T(1) - epsilon()) + { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } + else + { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } + } + + template + tquat rotate + ( + typename tquat::value_type angle, + tvec3 const & axis + ) + { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if(abs(len - T(1)) > T(0.001f)) + { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T)DEG2RAD(angle); + typename tquat::value_type const Sin = (T)sin(AngleRad * T(0.5)); + return tquat((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); + } + template + valType roll(tquat const & q) + { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); + } + + template + valType pitch(tquat const & q) + { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG); + } + template + valType yaw(tquat const & q) + { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); + } + + template + tvec3 eulerAngles(tquat const & x) + { + return tvec3(pitch(x), yaw(x), roll(x)); + } + + + template + tmat3x3 mat3_cast(const tquat& q) + { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); + } + + template + tmat4x4 mat4_cast(tquat const & q) + { + return tmat4x4(mat3_cast(q)); + } + template + tquat quat_cast(tmat3x3 const & m) + { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if(fourXSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if(fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if(fourZSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch(biggestIndex) + { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; + } + template + tquat quat_cast(tmat4x4 const & m4) + { + return quat_cast(tmat3x3(m4[0][0],m4[0][1],m4[0][2], + m4[1][0],m4[1][1],m4[1][2], + m4[2][0],m4[2][1],m4[2][2])); + } + + template + T angle(tquat const & x) + { + return acos(x.w) * T(2) * T(RAD2DEG); + } + + template + tvec3 axis(tquat const & x) + { + T tmp1 = T(1) - x.w * x.w; + if(tmp1 <= T(0)) + { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); + } + + + + template + tquat angleAxis(valType angle, tvec3 const & axis) + { + tquat result; + + valType a = (valType)(valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; + } + + + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////// + + template + tmat4x4 translate(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; + } + + template + tmat4x4 rotate + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; + } + + template + tmat4x4 scale(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; + } + + template + tmat4x4 rotate_slow + ( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) + { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; + } + + template + tmat4x4 scale_slow(tmat4x4 const & m,tvec3 const & v) + { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; + } + + template + tmat4x4 ortho + ( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) + { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = - valType(2) / (zFar - zNear); + res[3][0] = - (right + left) / (right - left); + res[3][1] = - (top + bottom) / (top - bottom); + res[3][2] = - (zFar + zNear) / (zFar - zNear); + return res; + } + + template + tmat4x4 frustum + ( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) + { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; + } + + template + tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) + { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = - (zFar + zNear) / (zFar - zNear); + res[2][3] = - valType(1); + res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear); + return res; + } + + template + tvec3 project + ( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tvec4 tmp = tvec4(obj.x, obj.y,obj.z,T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x,tmp.y,tmp.z); + } + + template + tvec3 unProject + ( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) + { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x,win.y,win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x,obj.y,obj.z); + } + + template + tmat4x4 pickMatrix + ( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) + { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if(!(delta.x > T(0) && delta.y > T(0))) + { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); + } + + template + tmat4x4 lookAt + ( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) + { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; + } + + template + class AxisAlignedBox2D + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() + { + _minimum = tvec2( T(-0.5), T(-0.5)); + _maximum = tvec2( T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox2D( const tvec2& min, const tvec2& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) + { + setExtents( tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) + { + return _minimum; + } + + void setMinimum( const tvec2& vec ) + { + _minimum = vec; + } + void setMinimum( T x,T y ) + { + _minimum = tvec2(x,y); + } + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) + { + return _maximum; + } + + + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec2& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y ) + { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec2& min, const tvec2& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) + { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + inline bool intersects(const AxisAlignedBox2D& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const + { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) + { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + inline void setNull() + { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() + { + _extent = EXTENT_INFINITE; + } + inline bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + + inline bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + + inline tvec2 getCenter(void) const + { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const + { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + inline bool operator== (const AxisAlignedBox2D& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + inline bool operator!= (const AxisAlignedBox2D& right) const + { + return !(*this == right); + } + + inline void merge(tvec2 point) + { + if (_minimum.x > point.x) + { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) + { + _minimum.y = point.y; + } + if (_maximum.x < point.x) + { + _maximum.x = point.x; + } + if (_maximum.y < point.y) + { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) + { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + + }; + + + template + class AxisAlignedBox + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; + public: + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum + { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() + { + _minimum = tvec3( T(-0.5), T(-0.5), T(-0.5) ); + _maximum = tvec3( T(0.5), T(0.5), T(0.5) ); + _extent = EXTENT_NULL; + } + AxisAlignedBox(const AxisAlignedBox & rkBox) + { + setExtents( rkBox._minimum, rkBox._maximum ); + _extent = rkBox._extent; + } + + AxisAlignedBox( const tvec3& min, const tvec3& max ) + { + setExtents( min, max ); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) + { + setExtents( mx, my, mz, Mx, My, Mz ); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) + { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() + { + } + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const + { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) + { + return _minimum; + } + + void setMinimum(const tvec3& mins) + { + _minimum = mins; + } + void setMinimum(T x,T y, T z) + { + _minimum = tvec3(x,y,z); + } + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const + { + return _maximum; + } + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) + { + return _maximum; + } + /** + * Sets the maximum corner of the box. + */ + void setMaximum( const tvec3& vec ) + { + _maximum = vec; + } + + void setMaximum( T x, T y, T z ) + { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX( T x ) + { + _maximum.x = x; + } + + void setMaximumY( T y ) + { + _maximum.y = y; + } + + void setMaximumZ( T z ) + { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents( const tvec3& min, const tvec3& max ) + { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz ) + { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+        1-----2
+        /|    /|
+        / |   / |
+        5-----4  |
+        |  0--|--3
+        | /   | /
+        |/    |/
+        6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8] ) const + { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const + { + switch(cornerToGet) + { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge( const AxisAlignedBox& right ) + { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) + { + return; + } + else if (right._extent == EXTENT_INFINITE) + { + _extent = EXTENT_INFINITE; + } + else if (_extent == EXTENT_NULL) + { + setExtents(right._minimum, right._maximum); + } + else + { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge( const tvec3& point ) + { + switch (_extent) + { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + void transform( const tmat4x4& matrix ) + { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert,vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge( currentCorner * matrix ); + + // min,min,max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // min max max + currentCorner.y = oldMax.y; + merge( currentCorner * matrix ); + + // min max min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix ); + + // max max min + currentCorner.x = oldMax.x; + merge( currentCorner * matrix ); + + // max max max + currentCorner.z = oldMax.z; + merge( currentCorner * matrix ); + + // max min max + currentCorner.y = oldMin.y; + merge( currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge( currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const + { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const + { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) + { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + void setNull() + { + _extent = EXTENT_NULL; + } + + bool isNull(void) const + { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const + { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() + { + _extent = EXTENT_INFINITE; + } + bool isInfinite(void) const + { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) + { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const + { + return( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y ); + } + + tvec3 getCenter(void) const + { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + /** + * Gets the size of the box + */ + tvec3 getSize(void) const + { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const + { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const + { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const + { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + bool operator== (const AxisAlignedBox& right) const + { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + bool operator!= (const AxisAlignedBox& right) const + { + return !(*this == right); + } + }; + + template + class tspline + { + public: + tspline() + { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + ~tspline(){}; + + void addPoint(const tvec3& p) + { + mPoints.push_back(p); + if (mAutoCalc) + { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + tvec3& getPoint(size_t index) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ȡ + */ + size_t getNumPoints(void) const + { + return mPoints.size(); + } + /** + * еĵ + */ + void clear(void) + { + mPoints.clear(); + mTangents.clear(); + } + /** + * µ + */ + void updatePoint(size_t index, const tvec3& value) + { + assert (index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) + { + recalcTangents(); + } + } + /** + * ֵȡ + */ + tvec3 interpolate(T time) const + { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned)fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + /** + * ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const + { + // Bounds check + assert (fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) + { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) + { + return mPoints[fromIndex]; + } + else if(t == 1.0f) + { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex+1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex+1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + /** + * Զ + */ + void setAutoCalculate(bool autoCalc) + { + mAutoCalc = autoCalc; + } + /** + * + */ + void recalcTangents(void) + { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) + { + return; + } + if (mPoints[0] == mPoints[numPoints-1]) + { + isClosed = true; + } + else + { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for(i = 0; i < numPoints; ++i) + { + if (i ==0) + { + // Special case start + if (isClosed) + { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]); + } + else + { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } + else if (i == numPoints-1) + { + if (isClosed) + { + mTangents[i] = mTangents[0]; + } + else + { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]); + } + } + else + { + mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]); + } + } + } + + public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; + }; + + + + + template < typename T > + class tellipsoidModel + { + public: + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR)) + { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator; + _eccentricitySquared= T(2)*flattening - flattening*flattening; + } + + ~tellipsoidModel(void) + { + } + + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const + { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + X = (N+height) * cos_latitude*cos(longitude); + Y = (N+height) * cos_latitude*sin(longitude); + Z = (N*(1-_eccentricitySquared)+height)*sin_latitude; + } + + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const + { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T)sqrt(X*X + Y*Y); + T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar)); + T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar); + + T sin_theta = (T)sin(theta); + T cos_theta = (T)cos(theta); + + latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) / + (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) ); + longitude = (T)atan2(Y,X); + + T sin_latitude = (T)sin(latitude); + T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude); + + height = p/(T)cos(latitude) - N; + } + + protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; + }; + class Rgba4Byte + { + public: + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) + { + _r = r; + _g = g; + _b = b; + _a = a; + } + Rgba4Byte(uint rgba) + { + _color = rgba; + } + friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r==right._r && + left._g==right._g && + left._b==right._b && + left._a==right._a; + } + friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right) + { + return left._r !=right._r || + left._g!=right._g || + left._b!=right._b || + left._a!=right._a; + } + + friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right) + { + return Rgba4Byte(left._r * right._r + ,left._g * right._g + ,left._b * right._b + ,left._a * right._a); + } + operator unsigned() + { + return _color; + } + uint toUint() + { + return _color; + } + public: + union + { + struct + { + + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; + }; + + typedef Rgba4Byte Rgba; + + inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) + { + Rgba4Byte color; + + color._r = (unsigned char)(c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char)(c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char)(c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char)(c1._a + s * (c2._a - c1._a)); + return color; + } + + inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) + { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; + } + + template + class tAxisAlignedBox2 + { + public: + enum Extent + { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; + public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; + public: + tvec2 center() const + { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const + { + return _vMax - _vMin; + } + + tvec2 halfSize() const + { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const + { + return( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y ); + } + + void merge(tvec2 point) + { + if (_vMin.x > point.x) + { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) + { + _vMin.y = point.y; + } + if (_vMax.x < point.x) + { + _vMax.x = point.x; + } + if (_vMax.y < point.y) + { + _vMax.y = point.y; + } + } + + + void merge(tAxisAlignedBox2 other) + { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const + { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y ; + } + + bool contains(tAxisAlignedBox2 other) const + { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y ; + } + }; + + template + class tray + { + typedef T value_type; + typedef tray type; + protected: + tvec3 _origin; + tvec3 _direction; + public: + tray(): + _origin(value_type(0),value_type(0),value_type(0)), + _direction(value_type(0),value_type(0),value_type(1)) + {} + tray(const tvec3& origin, const tvec3& direction): + _origin(origin), + _direction(direction) + {} + + /** + * ߵ + */ + void setOrigin(const tvec3& origin) + { + _origin = origin; + } + /** + * ߵ + */ + const tvec3& getOrigin(void) const + { + return _origin; + } + + /** + * ߵķ + */ + void setDirection(const tvec3& dir) + { + _direction = dir; + } + + /** + * ߵķ + */ + const tvec3& getDirection(void) const + { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const + { + return tvec3(_origin + (_direction * time)); + } + + /** + * boxཻ + * ཻ,ֵеfirst == true.false + * secondΪߵľ + * getPoint򷵻ؽ + */ + std::pair intersects(const AxisAlignedBox& box) const + { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ڰΧ + */ + if ( _origin > min && _origin < max ) + { + return std::pair(true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) + { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) + { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) + { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) + { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) + { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) + { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) + { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) + { + hit = true; + lowt = t; + } + } + } + return std::pair(hit, lowt); + } + }; + + + template + class Plane + { + public: + tvec3 _normal; + T _distance; + public: + Plane () + { + _normal = tvec3(0,0,0); + _distance = 0.0f; + } + Plane (const Plane& right) + { + _normal = right._normal; + _distance = right._distance; + } + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane (const tvec3& rkNormal, T fConstant) + { + _normal = rkNormal; + _distance = -fConstant; + } + /** Construct a plane using the 4 constants directly **/ + Plane (T x, T y, T z, T o) + { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + Plane (const tvec3& rkNormal, const tvec3& rkPoint) + { + redefine(rkNormal, rkPoint); + } + Plane (const tvec3& rkPoint0, const tvec3& rkPoint1,const tvec3& rkPoint2) + { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + /** + * ľ + */ + float distance(const tvec3 &pos) const + { + return dot(_normal,pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side + { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide (const tvec3& rkPoint) const + { + float fDistance = getDistance(rkPoint); + + if ( fDistance < 0.0 ) + return Plane::NEGATIVE_SIDE; + + if ( fDistance > 0.0 ) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + + Side getSide (const tvec3& centre, const tvec3& halfSize) const + { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance (const tvec3& rkPoint) const + { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) + { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1,kEdge2); + _normal.normalise(); + _distance = -dot(_normal,rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) + { + _normal = rkNormal; + _distance = -dot(rkNormal,rkPoint); + } + + +// tvec3 projectVector(const tvec3& p) const +// { +// matrix3 xform; +// xform[0][0] = 1.0f - _normal.x * _normal.x; +// xform[0][1] = -_normal.x * _normal.y; +// xform[0][2] = -_normal.x * _normal.z; +// xform[1][0] = -_normal.y * _normal.x; +// xform[1][1] = 1.0f - _normal.y * _normal.y; +// xform[1][2] = -_normal.y * _normal.z; +// xform[2][0] = -_normal.z * _normal.x; +// xform[2][1] = -_normal.z * _normal.y; +// xform[2][2] = 1.0f - _normal.z * _normal.z; +// return xform * p; +// } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) + { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) + { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + bool operator==(const Plane& right) const + { + return (right._distance == _distance && right._normal == _normal); + } + bool operator!=(const Plane& right) const + { + return (right._distance != _distance && right._normal != _normal); + } + }; + + template + class tfrustum + { + public: + enum + { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; + public: + /** + * project * modleview + */ + void loadFrustum(const tmat4x4 &mvp) + { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + bool pointInFrustum(const tvec3 &pos) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + bool sphereInFrustum(const tvec3 &pos, const float radius) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const + { + for (int i = 0; i < 6; i++) + { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const + { + return _planes[plane]; + } + protected: + Plane _planes[6]; + }; + + + typedef float real; + typedef tvec2 int2; + typedef tvec2 float2; + typedef tvec2 double2; + + typedef tvec2 real2; + + + typedef tvec3 int3; + typedef tvec3 uint3; + typedef tvec3 float3; + typedef tvec3 double3; + + typedef tvec3 real3; + + + typedef tvec4 int4; + typedef tvec4 float4; + typedef tvec4 double4; + typedef tvec4 real4; + typedef trect rect4; + typedef trect rect4i; + + typedef AxisAlignedBox aabb3d; + typedef AxisAlignedBox aabbr; + + typedef AxisAlignedBox2D AABB2D; + typedef AxisAlignedBox2D aabb2dr; + typedef AxisAlignedBox2D aabb2di; + + + + typedef tmat2x2 matrix2; + typedef tmat3x3 matrix3; + typedef tmat4x4 matrix4; + typedef tmat4x4 matrix4r; + + typedef tquat quaternion; + typedef tquat quatr; + typedef tray Ray; + typedef tfrustum Frustum; + + typedef tellipsoidModel ellipsoid; + +} diff --git a/example/lesson307-纹理包装-clamage/CELLTimestamp.hpp b/example/lesson307-纹理包装-clamage/CELLTimestamp.hpp new file mode 100644 index 0000000..5cbe804 --- /dev/null +++ b/example/lesson307-纹理包装-clamage/CELLTimestamp.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +namespace CELL +{ + class CELLTimestamp + { + public: + CELLTimestamp() + { + QueryPerformanceFrequency(&_frequency); + QueryPerformanceCounter(&_startCount); + } + ~CELLTimestamp() + {} + + void update() + { + QueryPerformanceCounter(&_startCount); + } + /** + * ȡǰ + */ + double getElapsedSecond() + { + return getElapsedTimeInMicroSec() * 0.000001; + } + /** + * ȡ + */ + double getElapsedTimeInMilliSec() + { + return this->getElapsedTimeInMicroSec() * 0.001; + } + /** + * ȡ΢ + */ + double getElapsedTimeInMicroSec() + { + LARGE_INTEGER endCount; + QueryPerformanceCounter(&endCount); + + double startTimeInMicroSec = _startCount.QuadPart * (1000000.0 / _frequency.QuadPart); + double endTimeInMicroSec = endCount.QuadPart * (1000000.0 / _frequency.QuadPart); + + return endTimeInMicroSec - startTimeInMicroSec; + } + protected: + LARGE_INTEGER _frequency; + LARGE_INTEGER _startCount; + }; +} diff --git a/example/lesson307-纹理包装-clamage/Image.hpp b/example/lesson307-纹理包装-clamage/Image.hpp new file mode 100644 index 0000000..53c987c --- /dev/null +++ b/example/lesson307-纹理包装-clamage/Image.hpp @@ -0,0 +1,80 @@ +#pragma once + + +namespace CELL +{ + class Image + { + protected: + int _width; + int _height; + uint* _pixel; + int _wrapType; + public: + Image(int w,int h,void* data) + { + _wrapType = 0; + if (w == 0 || h == 0 || data== 0) + { + _width = 0; + _height = 0; + _pixel = 0; + } + else + { + _width = w; + _height = h; + _pixel = new uint[w * h]; + memcpy(_pixel,data,w * h * sizeof(uint)); + } + } + ~Image() + { + delete []_pixel; + } + + + void setWrapType(int type) + { + _wrapType = type; + } + int width() const + { + return _width; + } + int height() const + { + return _height; + } + + Rgba pixelAt(int x,int y) const + { + return Rgba(_pixel[y * _width + x]); + } + + Rgba pixelUV(float u,float v) + { + float x = u * _width; + float y = v * _height; + if (_wrapType == 0) + { + return pixelAt((unsigned)(x)%_width,(unsigned)(y)%_height); + } + else + { + if (x >= _width) + { + x = _width - 1; + } + if (y >= _height) + { + y = _height - 1; + } + return pixelAt(x,y); + } + } + + public: + static Image* loadFromFile(const char*); + }; +} \ No newline at end of file diff --git a/example/lesson307-纹理包装-clamage/Raster.cpp b/example/lesson307-纹理包装-clamage/Raster.cpp new file mode 100644 index 0000000..b7387b4 --- /dev/null +++ b/example/lesson307-纹理包装-clamage/Raster.cpp @@ -0,0 +1,77 @@ + +#include "Raster.h" +#include "FreeImage.h" + +namespace CELL +{ + Image* Image::loadFromFile(const char* fileName) + { + //1 ȡͼƬʽ + FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); + if (fifmt == FIF_UNKNOWN) + { + return 0; + } + //2 ͼƬ + FIBITMAP *dib = FreeImage_Load(fifmt, fileName,0); + + FREE_IMAGE_COLOR_TYPE type = FreeImage_GetColorType(dib); + + //! ȡָ + FIBITMAP* temp = dib; + dib = FreeImage_ConvertTo32Bits(dib); + FreeImage_Unload(temp); + + BYTE* pixels = (BYTE*)FreeImage_GetBits(dib); + int width = FreeImage_GetWidth(dib); + int height = FreeImage_GetHeight(dib); + + int pitch = width*4; + BYTE* row = new BYTE[width*4]; + for(int j = 0; j < height / 2; j++) + { + memcpy(row,pixels + j * pitch,pitch ); + + memcpy(pixels + j * pitch,pixels + (height - j - 1) * pitch,pitch ); + + memcpy(pixels + (height - j - 1) * pitch,row,pitch ); + + } + delete []row; + + Image* image = new Image(width,height,pixels); + FreeImage_Unload(dib); + + return image; + } + + Raster::Raster( int w,int h,void* buffer ) + { + _texture = 0; + _width = w; + _height = h; + _buffer = (uint*)buffer; + memset(&_poitionPointer,0, sizeof(_poitionPointer)); + memset(&_colorPointer, 0, sizeof(_colorPointer)); + memset(&_uvPointer, 0, sizeof(_uvPointer)); + + _defaultColorPointer._size = 4; + _defaultColorPointer._type = DT_BYTE; + _defaultColorPointer._stride= sizeof(Rgba); + _defaultColorPointer._data = _defaultColorArray; + + _defaultUVPointer._size = 2; + _defaultUVPointer._type = DT_FLOAT; + _defaultUVPointer._stride = sizeof(float2); + _defaultUVPointer._data = _detaultUVArray; + } + + Raster::~Raster( void ) + { + } + + void Raster::clear() + { + memset(_buffer,0,_width * _height * sizeof(Rgba)); + } +} \ No newline at end of file diff --git a/example/lesson307-纹理包装-clamage/Raster.h b/example/lesson307-纹理包装-clamage/Raster.h new file mode 100644 index 0000000..cd5f942 --- /dev/null +++ b/example/lesson307-纹理包装-clamage/Raster.h @@ -0,0 +1,418 @@ +#pragma once + +#include "CELLMath.hpp" + +#include "Image.hpp" + +namespace CELL +{ + enum DRAWMODE + { + DM_POINTS = 0, + DM_LINES = 1, + DM_LINE_LOOP = 2, + DM_LINE_STRIP = 3, + DM_TRIANGES = 4, + }; + + enum DATETYPE + { + DT_BYTE, + DT_FLOAT, + DT_DOUBLE, + }; + + struct DateElementDes + { + int _size; + DATETYPE _type; + int _stride; + const void* _data; + }; + + class Span + { + public: + int _xStart; + int _xEnd; + Rgba _colorStart; + Rgba _colorEnd; + + float2 _uvStart; + float2 _uvEnd; + + int _y; + + public: + Span(int xStart,int xEnd,int y,Rgba colorStart,Rgba colorEnd,float2 uvStart,float2 uvEnd) + { + if (xStart < xEnd) + { + _xStart = xStart; + _xEnd = xEnd; + _colorStart = colorStart; + _colorEnd = colorEnd; + + _uvStart = uvStart; + _uvEnd = uvEnd; + + _y = y; + } + else + { + _xStart = xEnd; + _xEnd = xStart; + + _colorStart = colorEnd; + _colorEnd = colorStart; + + _uvStart = uvEnd; + _uvEnd = uvStart; + _y = y; + } + } + }; + + class Ege + { + public: + int _x1; + int _y1; + float2 _uv1; + Rgba _color1; + + int _x2; + int _y2; + float2 _uv2; + Rgba _color2; + + Ege(int x1,int y1,Rgba color1,float2 uv1,int x2,int y2,Rgba color2,float2 uv2) + { + if (y1 < y2) + { + _x1 = x1; + _y1 = y1; + _uv1 = uv1; + _color1 = color1; + + _x2 = x2; + _y2 = y2; + _uv2 = uv2; + _color2 = color2; + } + else + { + _x1 = x2; + _y1 = y2; + _uv1 = uv2; + _color1 = color2; + + _x2 = x1; + _y2 = y1; + _uv2 = uv1; + _color2 = color1; + } + } + }; + class Raster + { + public: + uint* _buffer; + int _width; + int _height; + Rgba _color; + + Image* _texture; + + DateElementDes _poitionPointer; + DateElementDes _colorPointer; + DateElementDes _uvPointer; + + DateElementDes _defaultColorPointer; + DateElementDes _defaultUVPointer; + Rgba _defaultColorArray[3]; + float2 _detaultUVArray[3]; + + + + public: + Raster(int w,int h,void* buffer); + ~Raster(void); + + void clear(); + + struct Vertex + { + int2 p0; + float2 uv0; + Rgba c0; + + int2 p1; + float2 uv1; + Rgba c1; + + int2 p2; + float2 uv2; + Rgba c2; + + }; + + void drawImage(int startX,int startY,const Image* image) + { + int left = tmax(startX,0); + int top = tmax(startY,0); + + int right = tmin(startX + image->width(),_width); + int bottom = tmin(startY + image->height(),_height); + + for (int x = left ; x < right ; ++ x) + { + for (int y = top ; y < bottom ; ++ y) + { + Rgba color = image->pixelAt(x - left,y - top); + setPixelEx(x,y,color); + } + } + } + public: + + + void vertexPointer(int size,DATETYPE type,int stride,const void* data) + { + _poitionPointer._size = size; + _poitionPointer._type = type; + _poitionPointer._stride = stride; + _poitionPointer._data = data; + } + + void colorPointer(int size,DATETYPE type,int stride,const void* data) + { + _colorPointer._size = size; + _colorPointer._type = type; + _colorPointer._stride = stride; + _colorPointer._data = data; + } + + void textureCoordPointer(int size,DATETYPE type,int stride,const void* data) + { + _uvPointer._size = size; + _uvPointer._type = type; + _uvPointer._stride = stride; + _uvPointer._data = data; + } + + void bindTexture(Image* image) + { + _texture = image; + } + + void drawArrays(DRAWMODE pri,int start,int count) + { + if (_poitionPointer._data == 0) + { + return; + } + + DateElementDes colorPointerdesc = _colorPointer; + DateElementDes uvPointerdesc = _uvPointer; + if (colorPointerdesc._data == 0) + { + colorPointerdesc = _defaultColorPointer; + } + if (uvPointerdesc._data == 0) + { + uvPointerdesc = _defaultUVPointer; + } + char* posData = (char*)_poitionPointer._data; + char* cData = (char*)colorPointerdesc._data; + char* uvData = (char*)uvPointerdesc._data; + + for(int i = start ;i < start + count; i += 3) + { + float* fData = (float*)posData; + int2 p0 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p1 (fData[0],fData[1]); + + posData += _poitionPointer._stride; + fData = (float*)(posData); + int2 p2 (fData[0],fData[1]); + posData += _poitionPointer._stride; + + + Rgba* pColor = (Rgba*)cData; + Rgba c0 (*pColor); + cData += _colorPointer._stride; + Rgba c1 (*pColor); + cData += _colorPointer._stride; + Rgba c2 (*pColor); + cData += _colorPointer._stride; + + float* pUV = (float*)uvData; + float2 uv0 (pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv1(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + pUV = (float*)uvData; + float2 uv2(pUV[0],pUV[1]); + uvData += _uvPointer._stride; + + + Ege eges[3] = + { + Ege(p0.x,p0.y,c0, uv0, p1.x,p1.y,c1, uv1), + Ege(p1.x,p1.y,c1, uv1, p2.x,p2.y,c2, uv2), + Ege(p2.x,p2.y,c2, uv2, p0.x,p0.y,c0, uv0), + }; + drawTrianle(eges); + + if (_colorPointer._data == 0) + { + cData = (char*)colorPointerdesc._data; + } + if (_uvPointer._data == 0 ) + { + + uvData = (char*)uvPointerdesc._data; + } + } + + } + + + void drawTrianle(Ege eges[3]) + { + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],_texture); + drawEge(eges[iMax],eges[iShort2],_texture); + } + void drawTriangle(const Vertex& vertex,Image* image) + { + Ege eges[3] = + { + Ege(vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0, vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1), + Ege(vertex.p1.x,vertex.p1.y,vertex.c1, vertex.uv1, vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2), + Ege(vertex.p2.x,vertex.p2.y,vertex.c2, vertex.uv2, vertex.p0.x,vertex.p0.y,vertex.c0, vertex.uv0), + }; + int iMax = 0; + int length = eges[0]._y2 - eges[0]._y1; + + for (int i = 1 ;i < 3 ; ++ i) + { + int len = eges[i]._y2 - eges[i]._y1; + if (len > length) + { + length = len; + iMax = i; + } + } + int iShort1 = (iMax + 1)%3; + int iShort2 = (iMax + 2)%3; + + drawEge(eges[iMax],eges[iShort1],image); + drawEge(eges[iMax],eges[iShort2],image); + + } + + void drawEge(const Ege& e1,const Ege& e2,Image* image) + { + float yOffset1 = e1._y2 - e1._y1; + if (yOffset1 == 0) + { + return; + } + + float yOffset = e2._y2 - e2._y1; + if (yOffset == 0) + { + return; + } + float xOffset = e2._x2 - e2._x1; + float scale = 0; + float step = 1.0f/yOffset; + + + float xOffset1 = e1._x2 - e1._x1; + float scale1 = (float)(e2._y1 - e1._y1)/yOffset1; + float step1 = 1.0f/yOffset1; + + for (int y = e2._y1 ; y < e2._y2 ; ++ y) + { + int x1 = e1._x1 + (int)(scale1 * xOffset1); + int x2 = e2._x1 + (int)(scale * xOffset); + Rgba color2 = colorLerp(e2._color1,e2._color2,scale); + Rgba color1 = colorLerp(e1._color1,e1._color2,scale1); + + float2 uvStart = uvLerp(e1._uv1,e1._uv2,scale1); + float2 uvEnd = uvLerp(e2._uv1,e2._uv2,scale); + + Span span(x1,x2,y,color1,color2,uvStart,uvEnd); + drawSpan(span,image); + + scale += step; + scale1 += step1; + + } + } + + void drawSpan(const Span& span,Image* image) + { + float length = span._xEnd - span._xStart; + float scale = 0; + float step = 1.0f/length; + for (int x = span._xStart ; x < span._xEnd; ++ x) + { + //Rgba color = colorLerp(span._colorStart,span._colorEnd,scale); + + float2 uv = uvLerp(span._uvStart,span._uvEnd,scale); + + Rgba pixel = image->pixelUV(uv.x,uv.y); + + //Rgba dst = color + pixel; + scale += step; + + setPixel(x,span._y,pixel); + } + } + void drawLine(float2 pt1,float2 pt2,Rgba color1,Rgba color2); + + void drawPoints(float2 pt1,Rgba4Byte color) + { + + } + inline void setPixelEx(unsigned x,unsigned y,Rgba color) + { + _buffer[y * _width + x] = color._color; + } + + inline Rgba getPixel(unsigned x,unsigned y) + { + return Rgba(_buffer[y * _width + x]); + } + inline void setPixel(unsigned x,unsigned y,Rgba color) + { + if (x >= _width || y >= _height) + { + return; + } + _buffer[y * _width + x] = color._color; + } + }; +} diff --git a/example/lesson307-纹理包装-clamage/lesson307-纹理包装-clamage.vcproj b/example/lesson307-纹理包装-clamage/lesson307-纹理包装-clamage.vcproj new file mode 100644 index 0000000..dc2ee40 --- /dev/null +++ b/example/lesson307-纹理包装-clamage/lesson307-纹理包装-clamage.vcproj @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example/lesson307-纹理包装-clamage/lesson307.cpp b/example/lesson307-纹理包装-clamage/lesson307.cpp new file mode 100644 index 0000000..24f5966 --- /dev/null +++ b/example/lesson307-纹理包装-clamage/lesson307.cpp @@ -0,0 +1,201 @@ +#include +#include + +#include "Raster.h" + +#include "CELLTimestamp.hpp" + + + +LRESULT CALLBACK windowProc(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam) +{ + switch(msg) + { + case WM_SIZE: + break; + case WM_CLOSE: + case WM_DESTROY: + PostQuitMessage(0); + break; + default: + break; + } + + return DefWindowProc( hWnd, msg, wParam, lParam ); +} + + +void getResourcePath(HINSTANCE hInstance,char pPath[1024]) +{ + char szPathName[1024]; + char szDriver[64]; + char szPath[1024]; + GetModuleFileNameA(hInstance,szPathName,sizeof(szPathName)); + _splitpath( szPathName, szDriver, szPath, 0, 0 ); + sprintf(pPath,"%s%s",szDriver,szPath); +} + +int WINAPI WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd ) +{ + // 1 עᴰ + ::WNDCLASSEXA winClass; + winClass.lpszClassName = "Raster"; + winClass.cbSize = sizeof(::WNDCLASSEX); + winClass.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC | CS_DBLCLKS; + winClass.lpfnWndProc = windowProc; + winClass.hInstance = hInstance; + winClass.hIcon = 0; + winClass.hIconSm = 0; + winClass.hCursor = LoadCursor(NULL, IDC_ARROW); + winClass.hbrBackground = (HBRUSH)(BLACK_BRUSH); + winClass.lpszMenuName = NULL; + winClass.cbClsExtra = 0; + winClass.cbWndExtra = 0; + RegisterClassExA(&winClass); + + // 2 + HWND hWnd = CreateWindowExA( + NULL, + "Raster", + "Raster", + WS_OVERLAPPEDWINDOW, + 0, + 0, + 800, + 600, + 0, + 0, + hInstance, + 0 + ); + + UpdateWindow( hWnd ); + ShowWindow(hWnd,SW_SHOW); + + RECT rt = {0}; + GetClientRect(hWnd,&rt); + + int width = rt.right - rt.left; + int height = rt.bottom - rt.top; + void* buffer = 0; + + HDC hDC = GetDC(hWnd); + HDC hMem = ::CreateCompatibleDC(hDC); + + BITMAPINFO bmpInfor; + bmpInfor.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); + bmpInfor.bmiHeader.biWidth = width; + bmpInfor.bmiHeader.biHeight = -height; + bmpInfor.bmiHeader.biPlanes = 1; + bmpInfor.bmiHeader.biBitCount = 32; + bmpInfor.bmiHeader.biCompression = BI_RGB; + bmpInfor.bmiHeader.biSizeImage = 0; + bmpInfor.bmiHeader.biXPelsPerMeter = 0; + bmpInfor.bmiHeader.biYPelsPerMeter = 0; + bmpInfor.bmiHeader.biClrUsed = 0; + bmpInfor.bmiHeader.biClrImportant = 0; + + HBITMAP hBmp = CreateDIBSection(hDC,&bmpInfor,DIB_RGB_COLORS,(void**)&buffer,0,0); + SelectObject(hMem,hBmp); + + char szPath[1024]; + getResourcePath(0,szPath); + + char szImage[1024]; + sprintf(szImage,"%s/image/bg.png",szPath); + CELL::Image* image = CELL::Image::loadFromFile(szImage); + + sprintf(szImage,"%s/image/scale.jpg",szPath); + CELL::Image* image1 = CELL::Image::loadFromFile(szImage); + + + CELL::Raster raster(width,height,buffer); + + + struct Vertex + { + float x,y; + float u,v; + CELL::Rgba color; + }; + + MSG msg = {0}; + while(true) + { + if (msg.message == WM_DESTROY + ||msg.message == WM_CLOSE + ||msg.message == WM_QUIT) + { + break; + } + if( PeekMessage( &msg, NULL, 0, 0, PM_REMOVE ) ) + { + TranslateMessage( &msg ); + DispatchMessage( &msg ); + } + raster.clear(); + + CELL::int2 pt[3] = + { + CELL::int2(100,10), + CELL::int2(10,100), + CELL::int2(200,100), + }; + + CELL::Rgba color1(255,0,0); + CELL::Rgba color2(0,255,0); + CELL::Rgba color3(0,0,255); + + CELL::CELLTimestamp tms; + + tms.update(); + + + double mis = tms.getElapsedTimeInMicroSec(); + + char szBuf[128]; + sprintf(szBuf,"%f ",mis); + int i = 00; + memcpy(buffer,raster._buffer,raster._width * raster._height * sizeof(CELL::Rgba)); + + raster.drawImage(0,0,image); + + + + + + Vertex vertexs[] = + { + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {210, 210, 2.0f, 2.0f, CELL::Rgba(255,255,255,255)}, + {210, 10, 2.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + + {10, 10, 0.0f, 0.0f, CELL::Rgba(255,255,255,255)}, + {210, 210, 2.0f, 2.0f, CELL::Rgba(255,255,255,255)}, + {10, 210, 0.0f, 2.0f, CELL::Rgba(255,255,255,255)}, + }; + + image1->setWrapType(1); + + raster.bindTexture(image1); + + raster.vertexPointer(2,CELL::DT_FLOAT, sizeof(Vertex),&vertexs[0].x); + raster.textureCoordPointer(2,CELL::DT_FLOAT,sizeof(Vertex),&vertexs[0].u); + //raster.colorPointer(4,CELL::DT_BYTE, sizeof(Vertex),&vertexs[0].color); + + raster.drawArrays(CELL::DM_TRIANGES,0,6); + + //raster.drawTriangle(vertex,image1); + //raster.drawTriangle(vertex1,image1); + + //TextOut(hMem,10,10,szBuf,strlen(szBuf)); + BitBlt(hDC,0,0,width,height,hMem,0,0,SRCCOPY); + + } + + + delete image; + delete image1; + + return 0; +} \ No newline at end of file diff --git a/example/lesson308-二维操作矩阵/1.ppt b/example/lesson308-二维操作矩阵/1.ppt new file mode 100644 index 0000000..e69de29