d/ns: Greatly extend the North Star driver · matrixfurry.com/monado@d4e034d

+5

src/xrt/drivers/CMakeLists.txt

··· 50 50 51 51 if(BUILD_DRIVER_NS) 52 52 set(NS_SOURCE_FILES 53 + north_star/distortion/utility_northstar.h 54 + north_star/distortion/deformation_northstar.h 55 + north_star/distortion/deformation_northstar.cpp 56 + north_star/ns_hmd.h 53 57 north_star/ns_hmd.c 54 58 north_star/ns_interface.h 55 59 north_star/ns_prober.c ··· 57 61 58 62 # Use OBJECT to not create a archive, since it just gets in the way. 59 63 add_library(drv_ns OBJECT ${NS_SOURCE_FILES}) 64 + target_include_directories(drv_ns SYSTEM PRIVATE ../../external) 60 65 list(APPEND ENABLED_HEADSET_DRIVERS ns) 61 66 endif() 62 67

+5 -1

src/xrt/drivers/meson.build

··· 42 42 lib_drv_ns = static_library( 43 43 'drv_ns', 44 44 files( 45 + 'north_star/distortion/utility_northstar.h', 46 + 'north_star/distortion/deformation_northstar.h', 47 + 'north_star/distortion/deformation_northstar.cpp', 48 + 'north_star/ns_hmd.h', 45 49 'north_star/ns_hmd.c', 46 50 'north_star/ns_interface.h', 47 51 'north_star/ns_prober.c', 48 52 ), 49 - include_directories: xrt_include, 53 + include_directories: [xrt_include, external_include], 50 54 dependencies: [aux, targets_enabled], 51 55 build_by_default: 'ns' in drivers, 52 56 )

+317

src/xrt/drivers/north_star/distortion/deformation_northstar.cpp

··· 1 + // Copyright 2020, Hesham Wahba. 2 + // Copyright 2020, Nova King. 3 + // SPDX-License-Identifier: BSD-3-Clause 4 + 5 + #include "deformation_northstar.h" 6 + 7 + OpticalSystem::OpticalSystem(const OpticalSystem &_in) 8 + { 9 + ellipseMinorAxis = _in.ellipseMinorAxis; 10 + ellipseMajorAxis = _in.ellipseMajorAxis; 11 + screenForward = _in.screenForward; 12 + screenPosition = _in.screenPosition; 13 + eyePosition = _in.eyePosition; 14 + worldToSphereSpace = _in.worldToSphereSpace; 15 + sphereToWorldSpace = _in.sphereToWorldSpace; 16 + worldToScreenSpace = _in.worldToScreenSpace; 17 + clipToWorld = _in.clipToWorld; 18 + cameraProjection = _in.cameraProjection; 19 + }; 20 + 21 + void 22 + OpticalSystem::LoadOpticalData(struct ns_eye *eye) 23 + { 24 + 25 + ellipseMinorAxis = eye->ellipse_minor_axis; 26 + ellipseMajorAxis = eye->ellipse_major_axis; 27 + 28 + screenForward.x = eye->screen_forward.x; 29 + screenForward.y = eye->screen_forward.y; 30 + screenForward.z = eye->screen_forward.z; 31 + 32 + screenPosition.x = eye->screen_position.x; 33 + screenPosition.y = eye->screen_position.y; 34 + screenPosition.z = eye->screen_position.z; 35 + 36 + eyePosition.x = eye->eye_pose.position.x; 37 + eyePosition.y = eye->eye_pose.position.y; 38 + eyePosition.z = eye->eye_pose.position.z; 39 + 40 + sphereToWorldSpace.m00 = eye->sphere_to_world_space.v[0]; 41 + sphereToWorldSpace.m01 = eye->sphere_to_world_space.v[1]; 42 + sphereToWorldSpace.m02 = eye->sphere_to_world_space.v[2]; 43 + sphereToWorldSpace.m03 = eye->sphere_to_world_space.v[3]; 44 + sphereToWorldSpace.m10 = eye->sphere_to_world_space.v[4]; 45 + sphereToWorldSpace.m11 = eye->sphere_to_world_space.v[5]; 46 + sphereToWorldSpace.m12 = eye->sphere_to_world_space.v[6]; 47 + sphereToWorldSpace.m13 = eye->sphere_to_world_space.v[7]; 48 + sphereToWorldSpace.m20 = eye->sphere_to_world_space.v[8]; 49 + sphereToWorldSpace.m21 = eye->sphere_to_world_space.v[9]; 50 + sphereToWorldSpace.m22 = eye->sphere_to_world_space.v[10]; 51 + sphereToWorldSpace.m23 = eye->sphere_to_world_space.v[11]; 52 + sphereToWorldSpace.m30 = 0.0f; 53 + sphereToWorldSpace.m31 = 0.0f; 54 + sphereToWorldSpace.m32 = 0.0f; 55 + sphereToWorldSpace.m33 = 1.0f; 56 + 57 + worldToScreenSpace.m00 = eye->world_to_screen_space.v[0]; 58 + worldToScreenSpace.m01 = eye->world_to_screen_space.v[1]; 59 + worldToScreenSpace.m02 = eye->world_to_screen_space.v[2]; 60 + worldToScreenSpace.m03 = eye->world_to_screen_space.v[3]; 61 + worldToScreenSpace.m10 = eye->world_to_screen_space.v[4]; 62 + worldToScreenSpace.m11 = eye->world_to_screen_space.v[5]; 63 + worldToScreenSpace.m12 = eye->world_to_screen_space.v[6]; 64 + worldToScreenSpace.m13 = eye->world_to_screen_space.v[7]; 65 + worldToScreenSpace.m20 = eye->world_to_screen_space.v[8]; 66 + worldToScreenSpace.m21 = eye->world_to_screen_space.v[9]; 67 + worldToScreenSpace.m22 = eye->world_to_screen_space.v[10]; 68 + worldToScreenSpace.m23 = eye->world_to_screen_space.v[11]; 69 + worldToScreenSpace.m30 = 0.0f; 70 + worldToScreenSpace.m31 = 0.0f; 71 + worldToScreenSpace.m32 = 0.0f; 72 + worldToScreenSpace.m33 = 1.0f; 73 + 74 + cameraProjection.w = eye->camera_projection.w; 75 + cameraProjection.x = eye->camera_projection.x; 76 + cameraProjection.y = eye->camera_projection.y; 77 + cameraProjection.z = eye->camera_projection.z; 78 + 79 + worldToSphereSpace = sphereToWorldSpace.Inverse(); 80 + 81 + UpdateClipToWorld(Matrix4x4::Identity()); 82 + }; 83 + 84 + void 85 + OpticalSystem::RegenerateMesh() 86 + { 87 + 88 + // m_logger->DriverLog("Regenerate mesh called."); 89 + 90 + std::map<float, std::map<float, Vector2>>::iterator outerIter; 91 + std::map<float, Vector2>::iterator innerIter; 92 + 93 + // walk all the requested stored UVs and regeneate the response values 94 + // you probably should have updated the eye position before doing this 95 + // :-D 96 + for (outerIter = m_requestedUVs.begin(); 97 + outerIter != m_requestedUVs.end(); outerIter++) { 98 + for (innerIter = outerIter->second.begin(); 99 + innerIter != outerIter->second.end(); innerIter++) { 100 + Vector2 result = SolveDisplayUVToRenderUV( 101 + Vector2(outerIter->first, innerIter->first), 102 + Vector2(innerIter->second.x, innerIter->second.y), 103 + m_iniSolverIters); 104 + innerIter->second.x = result.x; 105 + innerIter->second.y = result.y; 106 + } 107 + } 108 + } 109 + 110 + 111 + Vector2 112 + OpticalSystem::RenderUVToDisplayUV(Vector2 inputUV) 113 + { 114 + Vector3 rayDir; 115 + ViewportPointToRayDirection(inputUV, eyePosition, clipToWorld, rayDir); 116 + Vector2 curDisplayUV = RenderUVToDisplayUV(rayDir); 117 + return curDisplayUV; 118 + } 119 + 120 + Vector2 121 + OpticalSystem::RenderUVToDisplayUV(Vector3 inputUV) 122 + { 123 + 124 + Vector3 sphereSpaceRayOrigin = 125 + worldToSphereSpace.MultiplyPoint(eyePosition); 126 + Vector3 sphereSpaceRayDirection = 127 + (worldToSphereSpace.MultiplyPoint(eyePosition + inputUV) - 128 + sphereSpaceRayOrigin); 129 + sphereSpaceRayDirection = 130 + sphereSpaceRayDirection / sphereSpaceRayDirection.Magnitude(); 131 + 132 + float intersectionTime = 133 + intersectLineSphere(sphereSpaceRayOrigin, sphereSpaceRayDirection, 134 + Vector3::Zero(), 0.5f * 0.5f, false); 135 + 136 + if (intersectionTime < 0.f) { 137 + // m_logger->DriverLog("No line->ellipsoid intersection. %g %g", 138 + // inputUV.x, inputUV.y); 139 + return Vector2::zero(); 140 + } 141 + Vector3 sphereSpaceIntersection = 142 + sphereSpaceRayOrigin + (sphereSpaceRayDirection * intersectionTime); 143 + 144 + // Ellipsoid Normals 145 + Vector3 sphereSpaceNormal = 146 + (Vector3::Zero() - sphereSpaceIntersection) / 147 + sphereSpaceIntersection.Magnitude(); 148 + sphereSpaceNormal.x = 149 + sphereSpaceNormal.x / powf(ellipseMinorAxis / 2.f, 2.f); 150 + sphereSpaceNormal.y = 151 + sphereSpaceNormal.y / powf(ellipseMinorAxis / 2.f, 2.f); 152 + sphereSpaceNormal.z = 153 + sphereSpaceNormal.z / powf(ellipseMajorAxis / 2.f, 2.f); 154 + sphereSpaceNormal = sphereSpaceNormal / sphereSpaceNormal.Magnitude(); 155 + 156 + Vector3 worldSpaceIntersection = 157 + sphereToWorldSpace.MultiplyPoint(sphereSpaceIntersection); 158 + Vector3 worldSpaceNormal = 159 + sphereToWorldSpace.MultiplyVector(sphereSpaceNormal); 160 + worldSpaceNormal = worldSpaceNormal / worldSpaceNormal.Magnitude(); 161 + 162 + Ray firstBounce(worldSpaceIntersection, 163 + Vector3::Reflect(inputUV, worldSpaceNormal)); 164 + intersectionTime = 165 + intersectPlane(screenForward, screenPosition, firstBounce.m_Origin, 166 + firstBounce.m_Direction); 167 + 168 + if (intersectionTime < 0.f) { 169 + // m_logger->DriverLog("No bounce->screen intersection. %g %g", 170 + // inputUV.x, inputUV.y); 171 + return Vector2::zero(); 172 + } 173 + Vector3 planeIntersection = firstBounce.GetPoint(intersectionTime); 174 + 175 + Vector3 ScreenUVZ = 176 + worldToScreenSpace.MultiplyPoint3x4(planeIntersection); 177 + 178 + Vector2 ScreenUV; 179 + ScreenUV.x = ScreenUVZ.x; 180 + ScreenUV.y = ScreenUVZ.y; 181 + 182 + Vector2 ScreenUV_Real; 183 + 184 + ScreenUV_Real.y = 1.f - (ScreenUV.x + 0.5f); 185 + ScreenUV_Real.x = 1.f - (ScreenUV.y + 0.5f); 186 + 187 + return ScreenUV_Real; 188 + } 189 + 190 + 191 + Vector2 192 + OpticalSystem::SolveDisplayUVToRenderUV(Vector2 inputUV, 193 + Vector2 initailGuess, 194 + int iterations) 195 + { 196 + 197 + float epsilon = 0.0001f; 198 + Vector2 curCameraUV; 199 + curCameraUV.x = initailGuess.x; 200 + curCameraUV.y = initailGuess.y; 201 + Vector2 curDisplayUV; 202 + 203 + for (int i = 0; i < iterations; i++) { 204 + Vector3 rayDir; 205 + 206 + // we can do all three calls below to RenderUVToDisplayUV at the 207 + // same time via SIMD or better yet we can vectorize across all 208 + // the uvs if we have a list of them 209 + curDisplayUV = RenderUVToDisplayUV(curCameraUV); 210 + Vector2 displayUVGradX = 211 + (RenderUVToDisplayUV(curCameraUV + 212 + (Vector2(1, 0) * epsilon)) - 213 + curDisplayUV) / 214 + epsilon; 215 + Vector2 displayUVGradY = 216 + (RenderUVToDisplayUV(curCameraUV + 217 + (Vector2(0, 1) * epsilon)) - 218 + curDisplayUV) / 219 + epsilon; 220 + 221 + Vector2 error = curDisplayUV - inputUV; 222 + Vector2 step = Vector2::zero(); 223 + 224 + if (!displayUVGradX.x == 0.f || !displayUVGradX.y == 0.f) { 225 + step = step + (displayUVGradX * error.x); 226 + } 227 + if (!displayUVGradY.x == 0.f || !displayUVGradY.y == 0.f) { 228 + step = step + (displayUVGradY * error.y); 229 + } 230 + 231 + curCameraUV.x = curCameraUV.x - (step.x / 7.f); 232 + curCameraUV.y = curCameraUV.y - (step.y / 7.f); 233 + } 234 + 235 + return curCameraUV; 236 + } 237 + 238 + 239 + Vector2 240 + OpticalSystem::DisplayUVToRenderUVPreviousSeed(Vector2 inputUV) 241 + { 242 + // if we don't find a point we generate it and add it to our list 243 + Vector2 curDisplayUV; 244 + 245 + std::map<float, std::map<float, Vector2>>::iterator outerIter; 246 + outerIter = m_requestedUVs.find(inputUV.x); 247 + if (outerIter == m_requestedUVs.end()) { 248 + // if the outer value is not there we know the inner is not 249 + // so we just slam both in and call it a day 250 + std::map<float, Vector2> inner; 251 + curDisplayUV = SolveDisplayUVToRenderUV( 252 + inputUV, Vector2(0.5f, 0.5f), m_iniSolverIters); 253 + 254 + inner.insert( 255 + std::pair<float, Vector2>(inputUV.y, curDisplayUV)); 256 + m_requestedUVs.insert( 257 + std::pair<float, std::map<float, Vector2>>(inputUV.x, 258 + inner)); 259 + // Logger->DriverLog("NorthStar Generated UV %g %g ", 260 + // inputUV.x, inputUV.y); 261 + 262 + } else { 263 + std::map<float, Vector2>::iterator innerIter; 264 + innerIter = outerIter->second.find(inputUV.y); 265 + 266 + if (innerIter == outerIter->second.end()) { 267 + // we assume there is no reason to ask for the same 268 + // value so no need to check if it exists already so 269 + // just add it. 270 + curDisplayUV = SolveDisplayUVToRenderUV( 271 + inputUV, Vector2(0.5f, 0.5f), m_iniSolverIters); 272 + 273 + outerIter->second.insert( 274 + std::pair<float, Vector2>(inputUV.y, curDisplayUV)); 275 + // Logger->DriverLog("NorthStar Generated UV %g %g ", 276 + // outerIter->first, innerIter->first); 277 + } else { 278 + // return the value we found 279 + // hopefully we have remashed at least once otherwise 280 + // we are giving back the same points again 281 + // curDisplayUV.x = innerIter->second.x; 282 + // curDisplayUV.y = innerIter->second.y; 283 + 284 + curDisplayUV = SolveDisplayUVToRenderUV( 285 + inputUV, 286 + Vector2(innerIter->second.x, innerIter->second.y), 287 + m_optSolverIters); 288 + 289 + // Logger->DriverLog("NorthStar Found UV %g %g ", 290 + // outerIter->first, innerIter->first); 291 + } 292 + } 293 + return curDisplayUV; 294 + } 295 + 296 + 297 + extern "C" ns_optical_system * 298 + ns_create_optical_system(struct ns_eye *eye) 299 + { 300 + OpticalSystem *opticalSystem = new OpticalSystem(); 301 + opticalSystem->LoadOpticalData(eye); 302 + opticalSystem->setiters(50, 50); 303 + opticalSystem->RegenerateMesh(); 304 + return (ns_optical_system *)opticalSystem; 305 + } 306 + 307 + extern "C" void 308 + ns_display_uv_to_render_uv(struct ns_uv in, 309 + struct ns_uv *out, 310 + struct ns_eye eye) 311 + { 312 + OpticalSystem *opticalSystem = (OpticalSystem *)eye.optical_system; 313 + Vector2 inUV = Vector2(in.u, 1.f - in.v); 314 + Vector2 outUV = opticalSystem->DisplayUVToRenderUVPreviousSeed(inUV); 315 + out->u = outUV.x; 316 + out->v = outUV.y; 317 + }

+160

src/xrt/drivers/north_star/distortion/deformation_northstar.h

··· 1 + // Copyright 2020, Hesham Wahba. 2 + // Copyright 2020, Nova King. 3 + // SPDX-License-Identifier: BSD-3-Clause 4 + 5 + #pragma once 6 + 7 + #include <map> 8 + #include "utility_northstar.h" 9 + #include "../ns_hmd.h" 10 + #include <chrono> 11 + 12 + using namespace std::chrono; 13 + 14 + static char *s_strLeftEye = "leftEye"; 15 + static char *s_strRightEye = "rightEye"; 16 + 17 + class OpticalSystem 18 + { 19 + public: 20 + OpticalSystem(){}; 21 + 22 + OpticalSystem(const OpticalSystem &_in); 23 + 24 + void 25 + LoadOpticalData(struct ns_eye *eye); 26 + 27 + Vector3 28 + GetEyePosition() 29 + { 30 + return eyePosition; 31 + } 32 + 33 + Vector2 34 + RenderUVToDisplayUV(Vector3 inputUV); 35 + 36 + Vector2 37 + RenderUVToDisplayUV(Vector2 inputUV); 38 + 39 + Vector2 40 + SolveDisplayUVToRenderUV(Vector2 inputUV, 41 + Vector2 initailGuess, 42 + int iterations); 43 + 44 + Vector2 45 + DisplayUVToRenderUVPreviousSeed(Vector2 inputUV); 46 + 47 + void 48 + RegenerateMesh(); 49 + 50 + void 51 + UpdateEyePosition(const Vector3 pos) 52 + { 53 + eyePosition.x = pos.x; 54 + eyePosition.y = pos.y; 55 + eyePosition.z = pos.z; 56 + } 57 + 58 + const Vector4 59 + GetCameraProjection() 60 + { 61 + return cameraProjection; 62 + } 63 + 64 + void 65 + setiters(int init, int opt) 66 + { 67 + m_iniSolverIters = init; 68 + m_optSolverIters = opt; 69 + } 70 + 71 + void 72 + UpdateClipToWorld(Matrix4x4 eyeRotationMatrix) 73 + { 74 + // TEST DELETEME 75 + float time = clock() / (float)CLOCKS_PER_SEC; 76 + float osc = sinf(time * 2 * LP_PI); 77 + Matrix4x4 eyeToWorld = 78 + Matrix4x4::Translate(eyePosition) * eyeRotationMatrix; 79 + eyeToWorld.m02 *= -1; 80 + eyeToWorld.m12 *= -1; 81 + eyeToWorld.m22 *= -1; 82 + clipToWorld = 83 + eyeToWorld * cameraProjection.ComposeProjection().Inverse(); 84 + } 85 + 86 + Vector3 eyePosition; 87 + 88 + inline void 89 + ViewportPointToRayDirection(Vector2 UV, 90 + Vector3 cameraPosition, 91 + Matrix4x4 clipToWorld, 92 + Vector3 &out) 93 + { 94 + Vector3 tmp; 95 + tmp.x = UV.x - 0.5f; 96 + tmp.y = UV.y - 0.5f; 97 + tmp.z = 0.f; 98 + Vector3 dir = 99 + clipToWorld.MultiplyPoint(tmp * 2.f) - cameraPosition; 100 + 101 + float mag = dir.Magnitude(); 102 + out = dir / dir.Magnitude(); 103 + return; 104 + } 105 + 106 + private: 107 + float ellipseMinorAxis; 108 + float ellipseMajorAxis; 109 + Vector3 screenForward; 110 + Vector3 screenPosition; 111 + 112 + Vector4 cameraProjection; 113 + Matrix4x4 worldToSphereSpace; 114 + Matrix4x4 sphereToWorldSpace; 115 + Matrix4x4 worldToScreenSpace; 116 + Matrix4x4 clipToWorld; 117 + 118 + int m_iniSolverIters; 119 + int m_optSolverIters; 120 + 121 + std::map<float, std::map<float, Vector2> > m_requestedUVs; 122 + }; 123 + 124 + // supporting functions 125 + inline Vector3 126 + Project(Vector3 v1, Vector3 v2) 127 + { 128 + Vector3 v2Norm = (v2 / v2.Magnitude()); 129 + return v2Norm * Vector3::Dot(v1, v2Norm); 130 + } 131 + 132 + inline float 133 + intersectLineSphere(Vector3 Origin, 134 + Vector3 Direction, 135 + Vector3 spherePos, 136 + float SphereRadiusSqrd, 137 + bool frontSide = true) 138 + { 139 + Vector3 L = spherePos - Origin; 140 + Vector3 offsetFromSphereCenterToRay = Project(L, Direction) - L; 141 + return (offsetFromSphereCenterToRay.sqrMagnitude() <= SphereRadiusSqrd) 142 + ? Vector3::Dot(L, Direction) - 143 + (sqrt(SphereRadiusSqrd - 144 + offsetFromSphereCenterToRay.sqrMagnitude()) * 145 + (frontSide ? 1.f : -1.f)) 146 + : -1.f; 147 + } 148 + 149 + inline float 150 + intersectPlane(Vector3 n, Vector3 p0, Vector3 l0, Vector3 l) 151 + { 152 + 153 + float denom = Vector3::Dot((Vector3::Zero() - n), l); 154 + if (denom > 1.4e-45f) { 155 + Vector3 p0l0 = p0 - l0; 156 + float t = Vector3::Dot(p0l0, (Vector3::Zero() - n)) / denom; 157 + return t; 158 + } 159 + return -1.f; 160 + }

+28

src/xrt/drivers/north_star/distortion/ns_distortion.c

··· 1 + // Copyright 2020, Collabora, Ltd. 2 + // Copyright 2020, Nova King. 3 + // SPDX-License-Identifier: BSL-1.0 4 + /*! 5 + * @file 6 + * @brief North Star HMD code. 7 + * @author Nova King <technobaboo@gmail.com> 8 + * @author Jakob Bornecrantz <jakob@collabora.com> 9 + * @ingroup drv_ns 10 + */ 11 + 12 + 13 + #include <math.h> 14 + #include <stdio.h> 15 + #include <unistd.h> 16 + #include <stdlib.h> 17 + #include <string.h> 18 + #include <assert.h> 19 + 20 + #include "../ns_hmd.h" 21 + 22 + void 23 + ns_display_uv_to_render_uv(struct ns_uv display_uv, 24 + struct ns_uv *render_uv, 25 + struct ns_eye eye) 26 + { 27 + std::map<float, std::map<float, struct ns_uv> >::iterator outerIter; 28 + }

+1254

src/xrt/drivers/north_star/distortion/utility_northstar.h

··· 1 + // Copyright 2020, Hesham Wahba. 2 + // Copyright 2020, Nova King. 3 + // SPDX-License-Identifier: BSD-3-Clause 4 + 5 + #pragma once 6 + 7 + #include <math.h> 8 + using namespace std; 9 + 10 + // min set of functions needed 11 + const float LP_PI = 3.14159265f; // float error, will be 3.141592741f. 12 + const float kEpsilon = 0.00001f; 13 + 14 + 15 + // Min Port of a Vector3 class for the deformation rendering system. 16 + class Vector3 17 + { 18 + public: 19 + inline Vector3(const Vector3 &in) 20 + { 21 + x = in.x; 22 + y = in.y; 23 + z = in.z; 24 + }; 25 + 26 + inline Vector3() 27 + { 28 + x = 0.f; 29 + y = 0.f; 30 + z = 0.f; 31 + }; 32 + 33 + inline Vector3(float _x, float _y, float _z) 34 + { 35 + x = _x; 36 + y = _y; 37 + z = _z; 38 + }; 39 + 40 + inline Vector3(float _x, float _y) 41 + { 42 + x = _x; 43 + y = _y; 44 + z = 0.f; 45 + }; 46 + 47 + inline static Vector3 48 + Up() 49 + { 50 + return Vector3(0.f, 1.f, 0.f); 51 + }; 52 + inline static Vector3 53 + Down() 54 + { 55 + return Vector3(0.f, -1.f, 0.f); 56 + }; 57 + inline static Vector3 58 + Left() 59 + { 60 + return Vector3(-1.f, 0.f, 0.f); 61 + }; 62 + inline static Vector3 63 + Right() 64 + { 65 + return Vector3(1.f, 0.f, 0.f); 66 + }; 67 + inline static Vector3 68 + Forward() 69 + { 70 + return Vector3(0.f, 0.f, 1.f); 71 + }; 72 + inline static Vector3 73 + Backward() 74 + { 75 + return Vector3(0.f, 0.f, -1.f); 76 + }; 77 + 78 + inline Vector3 79 + operator-(Vector3 &rhs) 80 + { 81 + Vector3 ret; 82 + ret.x = (x - rhs.x); 83 + ret.y = (y - rhs.y); 84 + ret.z = (z - rhs.z); 85 + return ret; 86 + } 87 + 88 + inline Vector3 89 + operator-() 90 + { 91 + return Vector3(-x, -y, -z); 92 + } 93 + 94 + inline Vector3 95 + operator+(const Vector3 &rhs) 96 + { 97 + Vector3 ret; 98 + ret.x = (x + rhs.x); 99 + ret.y = (y + rhs.y); 100 + ret.z = (z + rhs.z); 101 + return ret; 102 + } 103 + 104 + inline Vector3 & 105 + operator+=(const Vector3 &v) 106 + { 107 + this->x += v.x; 108 + this->y += v.y; 109 + this->z += v.z; 110 + return *this; 111 + } 112 + 113 + inline Vector3 114 + operator/(const float &d) 115 + { 116 + Vector3 *ret = new Vector3(); 117 + ret->x = (x / d); 118 + ret->y = (y / d); 119 + ret->z = (z / d); 120 + return *ret; 121 + } 122 + 123 + inline Vector3 operator*(const float &d) 124 + { 125 + Vector3 ret; 126 + ret.x = (x * d); 127 + ret.y = (y * d); 128 + ret.z = (z * d); 129 + return ret; 130 + } 131 + 132 + inline Vector3 133 + Inverse() 134 + { 135 + Vector3 ret; 136 + ret.x = -x; 137 + ret.y = -y; 138 + ret.z = -z; 139 + return ret; 140 + } 141 + 142 + inline void 143 + copy(Vector3 &lhs, Vector3 rhs) 144 + { 145 + rhs.x = lhs.x; 146 + rhs.y = lhs.y; 147 + rhs.z = lhs.z; 148 + } 149 + 150 + inline float static Dot(Vector3 lhs, Vector3 rhs) 151 + { 152 + float result = 153 + (lhs.x * rhs.x) + (lhs.y * rhs.y) + (lhs.z * rhs.z); 154 + return result; 155 + } 156 + 157 + inline float 158 + Dot(Vector3 rhs) 159 + { 160 + float result = (x * rhs.x) + (y * rhs.y) + (z * rhs.z); 161 + return result; 162 + } 163 + 164 + inline static float 165 + Angle(Vector3 v0, Vector3 v1) 166 + { 167 + Vector3 dir0 = v0.Normalized(); 168 + Vector3 dir1 = v1.Normalized(); 169 + 170 + float dot = dir0.Dot(dir1); 171 + dot = (dot < -1.f ? -1.f : (dot > 1.f ? 1.f : dot)); 172 + 173 + float angle = acos(dot); 174 + 175 + return angle; 176 + } 177 + 178 + inline float 179 + sqrMagnitude() 180 + { 181 + return x * x + y * y + z * z; 182 + } 183 + 184 + inline float 185 + Magnitude() 186 + { 187 + return sqrt(x * x + y * y + z * z); 188 + } 189 + 190 + inline static Vector3 191 + Zero() 192 + { 193 + Vector3 ret; 194 + ret.x = 0.f; 195 + ret.y = 0.f; 196 + ret.z = 0.f; 197 + return ret; 198 + } 199 + 200 + inline static Vector3 201 + One() 202 + { 203 + Vector3 ret; 204 + ret.x = 1.f; 205 + ret.y = 1.f; 206 + ret.z = 1.f; 207 + return ret; 208 + } 209 + 210 + inline static Vector3 211 + Reflect(Vector3 inDirection, Vector3 inNormal) 212 + { 213 + return inNormal * -2.F * Dot(inNormal, inDirection) + 214 + inDirection; 215 + } 216 + 217 + inline void 218 + Normalize() 219 + { 220 + float mag = Magnitude(); 221 + if (mag > kEpsilon) { 222 + x = x / mag; 223 + y = y / mag; 224 + z = z / mag; 225 + } else { 226 + x = 0.f; 227 + y = 0.f; 228 + z = 0.f; 229 + } 230 + } 231 + 232 + inline Vector3 233 + Normalized() 234 + { 235 + Vector3 ret; 236 + float mag = Magnitude(); 237 + if (mag > kEpsilon) { 238 + ret.x = x / mag; 239 + ret.y = y / mag; 240 + ret.z = z / mag; 241 + } else { 242 + ret.x = 0.f; 243 + ret.y = 0.f; 244 + ret.z = 0.f; 245 + } 246 + return ret; 247 + } 248 + 249 + 250 + inline void 251 + rotate(Vector3 axis, float radians) 252 + { 253 + float cos_theta = cosf(radians); 254 + float sin_theta = sinf(radians); 255 + 256 + x = (x * cos_theta) + (Vector3::Cross(axis) * sin_theta).x + 257 + (axis.x * Vector3::Dot(axis, *this)) * (1 - cos_theta); 258 + 259 + y = (y * cos_theta) + (Vector3::Cross(axis) * sin_theta).y + 260 + (axis.y * Vector3::Dot(axis, *this)) * (1 - cos_theta); 261 + 262 + z = (z * cos_theta) + (Vector3::Cross(axis) * sin_theta).z + 263 + (axis.z * Vector3::Dot(axis, *this)) * (1 - cos_theta); 264 + } 265 + 266 + inline Vector3 267 + Cross(const Vector3 in) 268 + { 269 + Vector3 ret; 270 + ret.x = y * in.z - z * in.y; 271 + ret.y = z * in.x - x * in.z; 272 + ret.z = x * in.y - y * in.x; 273 + return ret; 274 + } 275 + 276 + float x; 277 + float y; 278 + float z; 279 + }; 280 + 281 + // Min Port of a Vector2 class for the deformation rendering system. 282 + class Vector2 283 + { 284 + public: 285 + inline Vector2() 286 + { 287 + x = 0.f; 288 + y = 0.f; 289 + }; 290 + 291 + inline Vector2(const Vector2 &in) 292 + { 293 + x = in.x; 294 + y = in.y; 295 + } 296 + 297 + inline Vector2(float _x, float _y) 298 + { 299 + x = _x; 300 + y = _y; 301 + }; 302 + 303 + inline static Vector2 304 + zero() 305 + { 306 + Vector2 ret; 307 + ret.x = 0.f; 308 + ret.y = 0.f; 309 + return ret; 310 + } 311 + 312 + inline static Vector2 313 + One() 314 + { 315 + Vector2 ret; 316 + ret.x = 1.f; 317 + ret.y = 1.f; 318 + return ret; 319 + } 320 + 321 + inline Vector2 322 + operator/(const float &d) 323 + { 324 + Vector2 ret; 325 + ret.x = (x / d); 326 + ret.y = (y / d); 327 + return ret; 328 + } 329 + 330 + inline Vector2 operator*(const float &d) 331 + { 332 + Vector2 ret; 333 + ret.x = (x * d); 334 + ret.y = (y * d); 335 + return ret; 336 + } 337 + inline Vector2 338 + operator-(const Vector2 &rhs) 339 + { 340 + Vector2 ret; 341 + ret.x = (x - rhs.x); 342 + ret.y = (y - rhs.y); 343 + return ret; 344 + } 345 + 346 + inline Vector2 347 + operator+(const Vector2 &rhs) 348 + { 349 + Vector2 ret; 350 + ret.x = (x + rhs.x); 351 + ret.y = (y + rhs.y); 352 + return ret; 353 + } 354 + 355 + float x; 356 + float y; 357 + }; 358 + 359 + class Matrix4x4 360 + { 361 + public: 362 + inline Matrix4x4() 363 + { 364 + m00 = 0.0f; 365 + m01 = 0.0f; 366 + m02 = 0.0f; 367 + m03 = 0.0f; 368 + m10 = 0.0f; 369 + m11 = 0.0f; 370 + m12 = 0.0f; 371 + m13 = 0.0f; 372 + m20 = 0.0f; 373 + m21 = 0.0f; 374 + m22 = 0.0f; 375 + m23 = 0.0f; 376 + m30 = 0.0f; 377 + m31 = 0.0f; 378 + m32 = 0.0f; 379 + m33 = 0.0f; 380 + }; 381 + 382 + inline Matrix4x4(const Matrix4x4 &_in) 383 + { 384 + m00 = _in.m00; 385 + m01 = _in.m01; 386 + m02 = _in.m02; 387 + m03 = _in.m03; 388 + m10 = _in.m10; 389 + m11 = _in.m11; 390 + m12 = _in.m12; 391 + m13 = _in.m13; 392 + m20 = _in.m20; 393 + m21 = _in.m21; 394 + m22 = _in.m22; 395 + m23 = _in.m23; 396 + m30 = _in.m30; 397 + m31 = _in.m31; 398 + m32 = _in.m32; 399 + m33 = _in.m33; 400 + }; 401 + 402 + inline Matrix4x4(float in00, 403 + float in01, 404 + float in02, 405 + float in03, 406 + float in10, 407 + float in11, 408 + float in12, 409 + float in13, 410 + float in20, 411 + float in21, 412 + float in22, 413 + float in23, 414 + float in30, 415 + float in31, 416 + float in32, 417 + float in33) 418 + { 419 + m00 = in00; 420 + m01 = in01; 421 + m02 = in02; 422 + m03 = in03; 423 + m10 = in10; 424 + m11 = in11; 425 + m12 = in12; 426 + m13 = in13; 427 + m20 = in20; 428 + m21 = in21; 429 + m22 = in22; 430 + m23 = in23; 431 + m30 = in30; 432 + m31 = in31; 433 + m32 = in32; 434 + m33 = in33; 435 + }; 436 + 437 + inline static Matrix4x4 438 + Identity() 439 + { 440 + Matrix4x4 m; 441 + m.m00 = 1; 442 + m.m01 = 0; 443 + m.m02 = 0; 444 + m.m03 = 0; 445 + m.m10 = 0; 446 + m.m11 = 1; 447 + m.m12 = 0; 448 + m.m13 = 0; 449 + m.m20 = 0; 450 + m.m21 = 0; 451 + m.m22 = 1; 452 + m.m23 = 0; 453 + m.m30 = 0; 454 + m.m31 = 0; 455 + m.m32 = 0; 456 + m.m33 = 1; 457 + return m; 458 + } 459 + 460 + // Returns a 3x3 rotation matrix (padded to a Matrix4x4). 461 + inline static Matrix4x4 462 + RotationAlign(Vector3 fromDir, Vector3 toDir) 463 + { 464 + const Vector3 v = fromDir.Cross(toDir); 465 + const float c = fromDir.Dot(toDir); 466 + const float k = 1.0f / (1.0f + c); 467 + 468 + return Matrix4x4(v.x * v.x * k + c, v.y * v.x * k - v.z, 469 + v.z * v.x * k + v.y, 0.f, v.x * v.y * k + v.z, 470 + v.y * v.y * k + c, v.z * v.y * k - v.x, 0.f, 471 + v.x * v.z * k - v.y, v.y * v.z * k + v.x, 472 + v.z * v.z * k + c, 0.f, 0.f, 0.f, 0.f, 1.f); 473 + } 474 + 475 + inline Matrix4x4 operator*(const Matrix4x4 &_in) 476 + { 477 + Matrix4x4 ret; 478 + ret.m00 = (m00 * _in.m00) + (m01 * _in.m10) + (m02 * _in.m20) + 479 + (m03 * _in.m30); 480 + ret.m01 = (m00 * _in.m01) + (m01 * _in.m11) + (m02 * _in.m21) + 481 + (m03 * _in.m31); 482 + ret.m02 = (m00 * _in.m02) + (m01 * _in.m12) + (m02 * _in.m22) + 483 + (m03 * _in.m32); 484 + ret.m03 = (m00 * _in.m03) + (m01 * _in.m13) + (m02 * _in.m23) + 485 + (m03 * _in.m33); 486 + 487 + 488 + ret.m10 = (m10 * _in.m00) + (m11 * _in.m10) + (m12 * _in.m20) + 489 + (m13 * _in.m30); 490 + ret.m11 = (m10 * _in.m01) + (m11 * _in.m11) + (m12 * _in.m21) + 491 + (m13 * _in.m31); 492 + ret.m12 = (m10 * _in.m02) + (m11 * _in.m12) + (m12 * _in.m22) + 493 + (m13 * _in.m32); 494 + ret.m13 = (m10 * _in.m03) + (m11 * _in.m13) + (m12 * _in.m23) + 495 + (m13 * _in.m33); 496 + 497 + 498 + ret.m20 = (m20 * _in.m00) + (m21 * _in.m10) + (m22 * _in.m20) + 499 + (m23 * _in.m30); 500 + ret.m21 = (m20 * _in.m01) + (m21 * _in.m11) + (m22 * _in.m21) + 501 + (m23 * _in.m31); 502 + ret.m22 = (m20 * _in.m02) + (m21 * _in.m12) + (m22 * _in.m22) + 503 + (m23 * _in.m32); 504 + ret.m23 = (m20 * _in.m03) + (m21 * _in.m13) + (m22 * _in.m23) + 505 + (m23 * _in.m33); 506 + 507 + ret.m30 = (m30 * _in.m00) + (m31 * _in.m10) + (m32 * _in.m20) + 508 + (m33 * _in.m30); 509 + ret.m31 = (m30 * _in.m01) + (m31 * _in.m11) + (m32 * _in.m21) + 510 + (m33 * _in.m31); 511 + ret.m32 = (m30 * _in.m02) + (m31 * _in.m12) + (m32 * _in.m22) + 512 + (m33 * _in.m32); 513 + ret.m33 = (m30 * _in.m03) + (m31 * _in.m13) + (m32 * _in.m23) + 514 + (m33 * _in.m33); 515 + 516 + return ret; 517 + } 518 + 519 + inline Vector3 520 + MultiplyPoint(Vector3 point) 521 + { 522 + Vector3 res; 523 + float w; 524 + res.x = m00 * point.x + m01 * point.y + m02 * point.z + m03; 525 + res.y = m10 * point.x + m11 * point.y + m12 * point.z + m13; 526 + res.z = m20 * point.x + m21 * point.y + m22 * point.z + m23; 527 + 528 + w = m30 * point.x + m31 * point.y + m32 * point.z + m33; 529 + 530 + w = 1.f / w; 531 + res.x *= w; 532 + res.y *= w; 533 + res.z *= w; 534 + return res; 535 + } 536 + 537 + inline static Matrix4x4 538 + Translate(Vector3 vector) 539 + { 540 + Matrix4x4 m; 541 + m.m00 = 1.f; 542 + m.m01 = 0.f; 543 + m.m02 = 0.f; 544 + m.m03 = vector.x; 545 + m.m10 = 0.f; 546 + m.m11 = 1.f; 547 + m.m12 = 0.f; 548 + m.m13 = vector.y; 549 + m.m20 = 0.f; 550 + m.m21 = 0.f; 551 + m.m22 = 1.f; 552 + m.m23 = vector.z; 553 + m.m30 = 0.f; 554 + m.m31 = 0.f; 555 + m.m32 = 0.f; 556 + m.m33 = 1.f; 557 + return m; 558 + } 559 + 560 + inline Vector3 561 + MultiplyVector(Vector3 vector) 562 + { 563 + Vector3 res; 564 + res.x = m00 * vector.x + m01 * vector.y + m02 * vector.z; 565 + res.y = m10 * vector.x + m11 * vector.y + m12 * vector.z; 566 + res.z = m20 * vector.x + m21 * vector.y + m22 * vector.z; 567 + return res; 568 + } 569 + 570 + inline Vector3 571 + MultiplyPoint3x4(Vector3 point) 572 + { 573 + Vector3 res; 574 + res.x = m00 * point.x + m01 * point.y + m02 * point.z + m03; 575 + res.y = m10 * point.x + m11 * point.y + m12 * point.z + m13; 576 + res.z = m20 * point.x + m21 * point.y + m22 * point.z + m23; 577 + return res; 578 + } 579 + 580 + inline Matrix4x4 581 + Transpose() 582 + { 583 + Matrix4x4 r; 584 + r.m00 = m00; 585 + r.m01 = m10; 586 + r.m02 = m20; 587 + r.m03 = m30; 588 + r.m10 = m01; 589 + r.m11 = m11; 590 + r.m12 = m21; 591 + r.m13 = m31; 592 + r.m20 = m02; 593 + r.m21 = m12; 594 + r.m22 = m22; 595 + r.m23 = m32; 596 + r.m30 = m03; 597 + r.m31 = m13; 598 + r.m32 = m23; 599 + r.m33 = m33; 600 + return r; 601 + } 602 + 603 + inline Matrix4x4 604 + Inverse() 605 + { 606 + float A2323 = m22 * m33 - m23 * m32; 607 + float A1323 = m21 * m33 - m23 * m31; 608 + float A1223 = m21 * m32 - m22 * m31; 609 + float A0323 = m20 * m33 - m23 * m30; 610 + float A0223 = m20 * m32 - m22 * m30; 611 + float A0123 = m20 * m31 - m21 * m30; 612 + float A2313 = m12 * m33 - m13 * m32; 613 + float A1313 = m11 * m33 - m13 * m31; 614 + float A1213 = m11 * m32 - m12 * m31; 615 + float A2312 = m12 * m23 - m13 * m22; 616 + float A1312 = m11 * m23 - m13 * m21; 617 + float A1212 = m11 * m22 - m12 * m21; 618 + float A0313 = m10 * m33 - m13 * m30; 619 + float A0213 = m10 * m32 - m12 * m30; 620 + float A0312 = m10 * m23 - m13 * m20; 621 + float A0212 = m10 * m22 - m12 * m20; 622 + float A0113 = m10 * m31 - m11 * m30; 623 + float A0112 = m10 * m21 - m11 * m20; 624 + 625 + float det = m00 * (m11 * A2323 - m12 * A1323 + m13 * A1223) - 626 + m01 * (m10 * A2323 - m12 * A0323 + m13 * A0223) + 627 + m02 * (m10 * A1323 - m11 * A0323 + m13 * A0123) - 628 + m03 * (m10 * A1223 - m11 * A0223 + m12 * A0123); 629 + det = 1 / det; 630 + 631 + return Matrix4x4( 632 + det * (m11 * A2323 - m12 * A1323 + m13 * A1223), 633 + det * -(m01 * A2323 - m02 * A1323 + m03 * A1223), 634 + det * (m01 * A2313 - m02 * A1313 + m03 * A1213), 635 + det * -(m01 * A2312 - m02 * A1312 + m03 * A1212), 636 + det * -(m10 * A2323 - m12 * A0323 + m13 * A0223), 637 + det * (m00 * A2323 - m02 * A0323 + m03 * A0223), 638 + det * -(m00 * A2313 - m02 * A0313 + m03 * A0213), 639 + det * (m00 * A2312 - m02 * A0312 + m03 * A0212), 640 + det * (m10 * A1323 - m11 * A0323 + m13 * A0123), 641 + det * -(m00 * A1323 - m01 * A0323 + m03 * A0123), 642 + det * (m00 * A1313 - m01 * A0313 + m03 * A0113), 643 + det * -(m00 * A1312 - m01 * A0312 + m03 * A0112), 644 + det * -(m10 * A1223 - m11 * A0223 + m12 * A0123), 645 + det * (m00 * A1223 - m01 * A0223 + m02 * A0123), 646 + det * -(m00 * A1213 - m01 * A0213 + m02 * A0113), 647 + det * (m00 * A1212 - m01 * A0212 + m02 * A0112)); 648 + } 649 + 650 + float m00; 651 + float m01; 652 + float m02; 653 + float m03; 654 + 655 + float m10; 656 + float m11; 657 + float m12; 658 + float m13; 659 + 660 + float m20; 661 + float m21; 662 + float m22; 663 + float m23; 664 + 665 + float m30; 666 + float m31; 667 + float m32; 668 + float m33; 669 + }; 670 + 671 + // Vector4 min port to get code working... 672 + class Vector4 673 + { 674 + public: 675 + inline Vector4() 676 + { 677 + x = 0.f; 678 + y = 0.f; 679 + z = 0.f; 680 + w = 0.f; 681 + } 682 + 683 + inline Vector4(const Vector4 &in) 684 + { 685 + x = in.x; 686 + y = in.y; 687 + z = in.z; 688 + w = in.w; 689 + } 690 + 691 + inline Matrix4x4 692 + ComposeProjection() 693 + { 694 + const float zNear = 0.07f; 695 + const float zFar = 1000.f; 696 + 697 + float fLeft = x; 698 + float fRight = y; 699 + float fTop = z; 700 + float fBottom = w; 701 + 702 + float idx = 1.0f / (fRight - fLeft); 703 + float idy = 1.0f / (fBottom - fTop); 704 + // float idz = 1.0f / (zFar - zNear); 705 + float sx = fRight + fLeft; 706 + float sy = fBottom + fTop; 707 + 708 + float c = -(zFar + zNear) / (zFar - zNear); 709 + float d = -(2.0F * zFar * zNear) / (zFar - zNear); 710 + 711 + Matrix4x4 m; 712 + m.m00 = 2.f * idx; 713 + m.m01 = 0.f; 714 + m.m02 = sx * idx; 715 + m.m03 = 0.f; 716 + m.m10 = 0.f; 717 + m.m11 = 2.f * idy; 718 + m.m12 = sy * idy; 719 + m.m13 = 0.f; 720 + m.m20 = 0.f; 721 + m.m21 = 0.f; 722 + m.m22 = c; 723 + m.m23 = d; 724 + m.m30 = 0.f; 725 + m.m31 = 0.f; 726 + m.m32 = -1.0f; 727 + m.m33 = 0.f; 728 + 729 + return m; 730 + } 731 + 732 + float x; 733 + float y; 734 + float z; 735 + float w; 736 + }; 737 + 738 + class Ray 739 + { 740 + public: 741 + inline Ray(Vector3 origin, Vector3 direction) 742 + { 743 + m_Origin = origin; 744 + direction.Normalize(); 745 + m_Direction.x = direction.x; 746 + m_Direction.y = direction.y; 747 + m_Direction.z = direction.z; 748 + } 749 + 750 + inline Vector3 751 + GetPoint(float distance) 752 + { 753 + return m_Origin + m_Direction * distance; 754 + } 755 + 756 + Vector3 m_Origin; 757 + Vector3 m_Direction; 758 + }; 759 + 760 + class Quaternion 761 + { 762 + public: 763 + inline Quaternion() 764 + { 765 + x = 0.f; 766 + y = 0.f; 767 + z = 0.f; 768 + w = 1.f; 769 + }; 770 + 771 + inline Quaternion(float _x, float _y, float _z, float _w) 772 + { 773 + x = _x; 774 + y = _y; 775 + z = _z; 776 + w = _w; 777 + }; 778 + 779 + inline Quaternion(const Quaternion &_in) 780 + { 781 + x = _in.x; 782 + y = _in.y; 783 + z = _in.z; 784 + w = _in.w; 785 + }; 786 + 787 + inline static Quaternion 788 + Identity() 789 + { 790 + return Quaternion(0, 0, 0, 1); 791 + } 792 + 793 + inline Quaternion 794 + conjugate() 795 + { 796 + return Quaternion(-x, -y, -z, w); 797 + } 798 + 799 + inline float 800 + norm() 801 + { 802 + return sqrt((x * x) + (y * y) + (z * z) + (w * w)); 803 + } 804 + 805 + inline Quaternion 806 + scale(float s) 807 + { 808 + return Quaternion(w * s, x * s, y * s, z * s); 809 + } 810 + 811 + inline Quaternion 812 + Inverse() 813 + { 814 + return conjugate() / norm(); 815 + } 816 + 817 + inline Vector3 818 + Right() 819 + { 820 + return *this * Vector3::Right(); 821 + } 822 + 823 + inline Vector3 824 + Up() 825 + { 826 + return *this * Vector3::Up(); 827 + } 828 + 829 + inline Vector3 830 + Forward() 831 + { 832 + return *this * Vector3::Forward(); 833 + } 834 + 835 + inline static void 836 + ToEulerAngle(const Quaternion &q, float &roll, float &pitch, float &yaw) 837 + { 838 + float sinr = +2.0f * (q.w * q.x + q.y * q.z); 839 + float cosr = +1.0f - 2.0f * (q.x * q.x + q.y * q.y); 840 + roll = atan2f(sinr, cosr); 841 + 842 + float sinp = +2.0f * (q.w * q.y - q.z * q.x); 843 + if (fabs(sinp) >= 1.f) { 844 + pitch = copysignf(LP_PI / 2.f, sinp); 845 + } else { 846 + pitch = asinf(sinp); 847 + } 848 + 849 + float siny = +2.0f * (q.w * q.z + q.x * q.y); 850 + float cosy = +1.0f - 2.0f * (q.y * q.y + q.z * q.z); 851 + yaw = atan2f(siny, cosy); 852 + } 853 + 854 + inline static Vector3 855 + ToEulerAngles(const Quaternion &in) 856 + { 857 + Vector3 euler; 858 + const static float PI_OVER_2 = LP_PI * 0.5f; 859 + const static float EPSILON = 1e-10f; 860 + float sqw, sqx, sqy, sqz; 861 + 862 + // quick conversion to Euler angles to give tilt to user 863 + sqw = in.w * in.w; 864 + sqx = in.x * in.x; 865 + sqy = in.y * in.y; 866 + sqz = in.z * in.z; 867 + 868 + euler.y = asinf(2.0f * (in.w * in.y - in.x * in.z)); 869 + if (PI_OVER_2 - fabs(euler.y) > EPSILON) { 870 + euler.z = atan2f(2.0f * (in.x * in.y + in.w * in.z), 871 + sqx - sqy - sqz + sqw); 872 + euler.x = atan2f(2.0f * (in.w * in.x + in.y * in.z), 873 + sqw - sqx - sqy + sqz); 874 + } else { 875 + // compute heading from local 'down' vector 876 + euler.z = atan2f(2.f * in.y * in.z - 2.f * in.x * in.w, 877 + 2.f * in.x * in.z + 2.f * in.y * in.w); 878 + euler.x = 0.0f; 879 + 880 + // If facing down, reverse yaw 881 + if (euler.y < 0.f) { 882 + euler.z = LP_PI - euler.z; 883 + } 884 + } 885 + return euler; 886 + } 887 + 888 + inline Vector3 operator*(Vector3 vec) 889 + { 890 + float num = x * 2.f; 891 + float num2 = y * 2.f; 892 + float num3 = z * 2.f; 893 + float num4 = x * num; 894 + float num5 = y * num2; 895 + float num6 = z * num3; 896 + float num7 = x * num2; 897 + float num8 = x * num3; 898 + float num9 = y * num3; 899 + float num10 = w * num; 900 + float num11 = w * num2; 901 + float num12 = w * num3; 902 + Vector3 result; 903 + result.x = (1.f - (num5 + num6)) * vec.x + 904 + (num7 - num12) * vec.y + (num8 + num11) * vec.z; 905 + result.y = (num7 + num12) * vec.x + 906 + (1.f - (num4 + num6)) * vec.y + 907 + (num9 - num10) * vec.z; 908 + result.z = (num8 - num11) * vec.x + (num9 + num10) * vec.y + 909 + (1.f - (num4 + num5)) * vec.z; 910 + return result; 911 + } 912 + 913 + inline static Quaternion 914 + Euler(const Vector3 &euler) 915 + { 916 + float c1 = cos(euler.z * 0.5f); 917 + float c2 = cos(euler.y * 0.5f); 918 + float c3 = cos(euler.x * 0.5f); 919 + float s1 = sin(euler.z * 0.5f); 920 + float s2 = sin(euler.y * 0.5f); 921 + float s3 = sin(euler.x * 0.5f); 922 + 923 + Quaternion ret; 924 + ret.x = c1 * c2 * s3 - s1 * s2 * c3; 925 + ret.y = c1 * s2 * c3 + s1 * c2 * s3; 926 + ret.z = s1 * c2 * c3 - c1 * s2 * s3; 927 + ret.w = c1 * c2 * c3 + s1 * s2 * s3; 928 + 929 + return ret; 930 + } 931 + 932 + 933 + Matrix4x4 934 + ToMatrix4x4() 935 + { 936 + float qw = w; 937 + float qx = x; 938 + float qy = y; 939 + float qz = z; 940 + 941 + const float n = 942 + 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); 943 + qx *= n; 944 + qy *= n; 945 + qz *= n; 946 + qw *= n; 947 + 948 + return Matrix4x4(1.0f - 2.0f * qy * qy - 2.0f * qz * qz, 949 + 2.0f * qx * qy - 2.0f * qz * qw, 950 + 2.0f * qx * qz + 2.0f * qy * qw, 0.0f, 951 + 2.0f * qx * qy + 2.0f * qz * qw, 952 + 1.0f - 2.0f * qx * qx - 2.0f * qz * qz, 953 + 2.0f * qy * qz - 2.0f * qx * qw, 0.0f, 954 + 2.0f * qx * qz - 2.0f * qy * qw, 955 + 2.0f * qy * qz + 2.0f * qx * qw, 956 + 1.0f - 2.0f * qx * qx - 2.0f * qy * qy, 0.0f, 957 + 0.0f, 0.0f, 0.0f, 1.0f); 958 + } 959 + 960 + inline Quaternion operator*(const Quaternion &q) 961 + { 962 + return Quaternion(y * q.z - z * q.y + x * q.w + w * q.x, 963 + z * q.x - x * q.z + y * q.w + w * q.y, 964 + x * q.y - y * q.x + z * q.w + w * q.z, 965 + w * q.w - x * q.x - y * q.y - z * q.z); 966 + } 967 + 968 + Quaternion 969 + operator/(const float div) 970 + { 971 + return Quaternion(x / div, y / div, z / div, w / div); 972 + } 973 + 974 + inline static Quaternion 975 + AxisAngle(Vector3 axis, float angle) 976 + { 977 + float halfAngle = angle * .5f; 978 + float s = (float)sin(halfAngle); 979 + Quaternion q; 980 + q.x = axis.x * s; 981 + q.y = axis.y * s; 982 + q.z = axis.z * s; 983 + q.w = (float)cos(halfAngle); 984 + return q; 985 + } 986 + 987 + inline static Quaternion 988 + LookAt(Vector3 sourcePoint, Vector3 destPoint) 989 + { 990 + Vector3 forwardVector = (destPoint - sourcePoint).Normalized(); 991 + 992 + float dot = Vector3::Dot(Vector3::Forward(), forwardVector); 993 + 994 + if (fabs(dot - (-1.0f)) < 0.000001f) { 995 + return Quaternion(Vector3::Up().x, Vector3::Up().y, 996 + Vector3::Up().z, 3.1415926535897932f); 997 + } 998 + if (fabs(dot - (1.0f)) < 0.000001f) { 999 + return Quaternion(); 1000 + } 1001 + 1002 + float rotAngle = cos(dot); 1003 + Vector3 rotAxis = Vector3::Forward().Cross(forwardVector); 1004 + rotAxis = rotAxis.Normalized(); 1005 + return AxisAngle(rotAxis, rotAngle); 1006 + } 1007 + 1008 + inline static Quaternion 1009 + QuaternionLookRotation(Vector3 forward, Vector3 Up) 1010 + { 1011 + 1012 + Vector3 vector1 = forward.Normalized(); 1013 + Vector3 vector2 = (Up.Cross(vector1)).Normalized(); 1014 + Vector3 vector3 = vector1.Cross(vector2); 1015 + 1016 + float m00 = vector2.x; 1017 + float m01 = vector2.y; 1018 + float m02 = vector2.z; 1019 + float m10 = vector3.x; 1020 + float m11 = vector3.y; 1021 + float m12 = vector3.z; 1022 + float m20 = vector1.x; 1023 + float m21 = vector1.y; 1024 + float m22 = vector1.z; 1025 + 1026 + float num8 = (m00 + m11) + m22; 1027 + Quaternion quaternion; 1028 + if (num8 > 0.f) { 1029 + float num = (float)sqrtf(num8 + 1.f); 1030 + quaternion.w = num * 0.5f; 1031 + num = 0.5f / num; 1032 + quaternion.x = (m12 - m21) * num; 1033 + quaternion.y = (m20 - m02) * num; 1034 + quaternion.z = (m01 - m10) * num; 1035 + return quaternion; 1036 + } 1037 + 1038 + if ((m00 >= m11) && (m00 >= m22)) { 1039 + float num7 = (float)sqrtf(((1.f + m00) - m11) - m22); 1040 + float num4 = 0.5f / num7; 1041 + quaternion.x = 0.5f * num7; 1042 + quaternion.y = (m01 + m10) * num4; 1043 + quaternion.z = (m02 + m20) * num4; 1044 + quaternion.w = (m12 - m21) * num4; 1045 + return quaternion; 1046 + } 1047 + 1048 + if (m11 > m22) { 1049 + float num6 = (float)sqrtf(((1.f + m11) - m00) - m22); 1050 + float num3 = 0.5f / num6; 1051 + quaternion.x = (m10 + m01) * num3; 1052 + quaternion.y = 0.5f * num6; 1053 + quaternion.z = (m21 + m12) * num3; 1054 + quaternion.w = (m20 - m02) * num3; 1055 + return quaternion; 1056 + } 1057 + 1058 + float num5 = (float)sqrtf(((1.f + m22) - m00) - m11); 1059 + float num2 = 0.5f / num5; 1060 + quaternion.x = (m20 + m02) * num2; 1061 + quaternion.y = (m21 + m12) * num2; 1062 + quaternion.z = 0.5f * num5; 1063 + quaternion.w = (m01 - m10) * num2; 1064 + return quaternion; 1065 + } 1066 + 1067 + 1068 + inline float 1069 + SIGN(float x) 1070 + { 1071 + return (x >= 0.0f) ? +1.0f : -1.0f; 1072 + } 1073 + 1074 + inline float 1075 + NORM(float a, float b, float c, float d) 1076 + { 1077 + return sqrt(a * a + b * b + c * c + d * d); 1078 + } 1079 + 1080 + inline static Quaternion 1081 + FromMatrix(const Matrix4x4 m) 1082 + { 1083 + float tr = m.m00 + m.m11 + m.m22; 1084 + float qx, qy, qz, qw; 1085 + 1086 + if (tr > 0) { 1087 + float S = sqrtf(tr + 1.f) * 2.f; 1088 + qw = 0.25f * S; 1089 + qx = (m.m21 - m.m12) / S; 1090 + qy = (m.m02 - m.m20) / S; 1091 + qz = (m.m10 - m.m01) / S; 1092 + } else if ((m.m00 > m.m11) && (m.m00 > m.m22)) { 1093 + float S = sqrtf(1.f + m.m00 - m.m11 - m.m22) * 2.f; 1094 + qw = (m.m21 - m.m12) / S; 1095 + qx = 0.25f * S; 1096 + qy = (m.m01 + m.m10) / S; 1097 + qz = (m.m02 + m.m20) / S; 1098 + } else if (m.m11 > m.m22) { 1099 + float S = sqrtf(1.f + m.m11 - m.m00 - m.m22) * 2.f; 1100 + qw = (m.m02 - m.m20) / S; 1101 + qx = (m.m01 + m.m10) / S; 1102 + qy = 0.25f * S; 1103 + qz = (m.m12 + m.m21) / S; 1104 + } else { 1105 + float S = sqrtf(1.f + m.m22 - m.m00 - m.m11) * 2.f; 1106 + qw = (m.m10 - m.m01) / S; 1107 + qx = (m.m02 + m.m20) / S; 1108 + qy = (m.m12 + m.m21) / S; 1109 + qz = 0.25f * S; 1110 + } 1111 + 1112 + return Quaternion(qx, qy, qz, qw); 1113 + } 1114 + 1115 + //// quaternion = [w, x, y, z]' 1116 + // void mRot2Quat(const Matrix4x4 m) { 1117 + // float r11 = m.m00; 1118 + // float r12 = m.m01; 1119 + // float r13 = m.m02; 1120 + // float r21 = m.m10; 1121 + // float r22 = m.m11; 1122 + // float r23 = m.m12; 1123 + // float r31 = m.m20; 1124 + // float r32 = m.m21; 1125 + // float r33 = m.m22; 1126 + // float w = (r11 + r22 + r33 + 1.0f) / 4.0f; 1127 + // float x = (r11 - r22 - r33 + 1.0f) / 4.0f; 1128 + // float y = (-r11 + r22 - r33 + 1.0f) / 4.0f; 1129 + // float z = (-r11 - r22 + r33 + 1.0f) / 4.0f; 1130 + // if (w < 0.0f) { 1131 + // w = 0.0f; 1132 + // } 1133 + // if (x < 0.0f) { 1134 + // x = 0.0f; 1135 + // } 1136 + // if (y < 0.0f) { 1137 + // y = 0.0f; 1138 + // } 1139 + // if (z < 0.0f) { 1140 + // z = 0.0f; 1141 + // } 1142 + // w = sqrt(w); 1143 + // x = sqrt(x); 1144 + // y = sqrt(y); 1145 + // z = sqrt(z); 1146 + // if (w >= x && w >= y && w >= z) { 1147 + // w *= +1.0f; 1148 + // x *= SIGN(r32 - r23); 1149 + // y *= SIGN(r13 - r31); 1150 + // z *= SIGN(r21 - r12); 1151 + // } 1152 + // else if (x >= w && x >= y && x >= z) { 1153 + // w *= SIGN(r32 - r23); 1154 + // x *= +1.0f; 1155 + // y *= SIGN(r21 + r12); 1156 + // z *= SIGN(r13 + r31); 1157 + // } 1158 + // else if (y >= w && y >= x && y >= z) { 1159 + // w *= SIGN(r13 - r31); 1160 + // x *= SIGN(r21 + r12); 1161 + // y *= +1.0f; 1162 + // z *= SIGN(r32 + r23); 1163 + // } 1164 + // else if (z >= w && z >= x && z >= y) { 1165 + // w *= SIGN(r21 - r12); 1166 + // x *= SIGN(r31 + r13); 1167 + // y *= SIGN(r32 + r23); 1168 + // z *= +1.0f; 1169 + // } 1170 + // else { 1171 + // //printf("coding error\n"); 1172 + // } 1173 + // float r = NORM(w, x, w, z); 1174 + // w /= r; 1175 + // x /= r; 1176 + // y /= r; 1177 + // z /= r; 1178 + //} 1179 + 1180 + inline static Quaternion 1181 + FromToRotation(Vector3 dir0, Vector3 dir1) 1182 + { 1183 + Vector3 axis = dir0.Cross(dir1).Normalized(); 1184 + float angle = Vector3::Angle(dir0, dir1); 1185 + return Quaternion::AxisAngle(axis, angle); 1186 + } 1187 + 1188 + float x; 1189 + float y; 1190 + float z; 1191 + float w; 1192 + }; 1193 + 1194 + // A position and rotation. You can multiply two poses; this acts like 1195 + // Matrix4x4 multiplication, but Poses always have unit scale. 1196 + class Pose 1197 + { 1198 + public: 1199 + Vector3 position; 1200 + Quaternion rotation; 1201 + 1202 + Pose(Vector3 pos) 1203 + { 1204 + position = pos; 1205 + rotation = Quaternion::Identity(); 1206 + } 1207 + Pose(Quaternion rot) 1208 + { 1209 + position = Vector3::Zero(); 1210 + rotation = rot; 1211 + } 1212 + Pose(Vector3 pos, Quaternion rot) 1213 + { 1214 + position = pos; 1215 + rotation = rot; 1216 + } 1217 + 1218 + inline static Pose 1219 + Identity() 1220 + { 1221 + return Pose(Vector3::Zero(), Quaternion::Identity()); 1222 + } 1223 + 1224 + inline Pose 1225 + Inverse() 1226 + { 1227 + Quaternion invQ = rotation.Inverse(); 1228 + return Pose(invQ * -position, invQ); 1229 + } 1230 + 1231 + inline Matrix4x4 1232 + Matrix() 1233 + { 1234 + return Matrix4x4::Translate(position) * rotation.ToMatrix4x4(); 1235 + } 1236 + 1237 + inline Pose operator*(Pose rhs) 1238 + { 1239 + return Pose(position + (rotation * rhs.position), 1240 + rotation * rhs.rotation); 1241 + } 1242 + 1243 + inline Pose operator*(Vector3 rhs) 1244 + { 1245 + return Pose(position + rotation * rhs, rotation); 1246 + } 1247 + 1248 + inline static Pose 1249 + FromMatrix(Matrix4x4 m) 1250 + { 1251 + return Pose(Vector3(m.m03, m.m13, m.m23), 1252 + Quaternion::FromMatrix(m)); 1253 + } 1254 + };

+202 -98

src/xrt/drivers/north_star/ns_hmd.c

··· 17 17 #include <string.h> 18 18 #include <assert.h> 19 19 20 - #include "math/m_api.h" 21 - #include "xrt/xrt_device.h" 20 + #include "ns_hmd.h" 21 + 22 22 #include "util/u_var.h" 23 - #include "util/u_misc.h" 24 23 #include "util/u_debug.h" 25 24 #include "util/u_device.h" 26 25 #include "util/u_time.h" 27 26 #include "util/u_distortion_mesh.h" 28 27 29 28 #include "targets_enabled_drivers.h" 29 + #ifdef XRT_BUILD_DRIVER_RS 30 30 #include "../realsense/rs_interface.h" 31 - 32 - 33 - /* 34 - * 35 - * Structs and defines. 36 - * 37 - */ 38 - 39 - struct ns_hmd 40 - { 41 - struct xrt_device base; 42 - 43 - struct xrt_device *tracker; 44 - 45 - struct xrt_pose pose; 46 - 47 - bool print_spew; 48 - bool print_debug; 49 - }; 50 - 51 - struct ns_mesh 52 - { 53 - struct u_uv_generator base; 54 - }; 31 + #endif 55 32 56 33 57 34 /* ··· 60 37 * 61 38 */ 62 39 63 - static inline struct ns_hmd * 64 - ns_hmd(struct xrt_device *xdev) 40 + struct ns_hmd * 41 + get_ns_hmd(struct xrt_device *xdev) 65 42 { 66 43 return (struct ns_hmd *)xdev; 67 44 } 68 45 69 - #define NS_SPEW(c, ...) \ 70 - do { \ 71 - if (c->print_spew) { \ 72 - fprintf(stderr, "%s - ", __func__); \ 73 - fprintf(stderr, __VA_ARGS__); \ 74 - fprintf(stderr, "\n"); \ 75 - } \ 76 - } while (false) 77 - 78 - #define NS_DEBUG(c, ...) \ 79 - do { \ 80 - if (c->print_debug) { \ 81 - fprintf(stderr, "%s - ", __func__); \ 82 - fprintf(stderr, __VA_ARGS__); \ 83 - fprintf(stderr, "\n"); \ 84 - } \ 85 - } while (false) 86 - 87 - #define NS_ERROR(c, ...) \ 88 - do { \ 89 - fprintf(stderr, "%s - ", __func__); \ 90 - fprintf(stderr, __VA_ARGS__); \ 91 - fprintf(stderr, "\n"); \ 92 - } while (false) 46 + static inline struct ns_mesh * 47 + ns_mesh(struct u_uv_generator *gen) 48 + { 49 + return (struct ns_mesh *)gen; 50 + } 93 51 94 52 static void 95 53 ns_hmd_destroy(struct xrt_device *xdev) 96 54 { 97 - struct ns_hmd *ns = ns_hmd(xdev); 98 - 99 - if (ns->tracker != NULL) { 100 - ns->tracker->destroy(ns->tracker); 101 - ns->tracker = NULL; 102 - } 55 + struct ns_hmd *ns = get_ns_hmd(xdev); 103 56 104 57 // Remove the variable tracking. 105 58 u_var_remove_root(ns); ··· 110 63 static void 111 64 ns_hmd_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) 112 65 { 113 - struct ns_hmd *ns = ns_hmd(xdev); 66 + struct ns_hmd *ns = get_ns_hmd(xdev); 114 67 115 - // Also update the tracking module if it is in use. 116 68 if (ns->tracker != NULL) { 117 69 ns->tracker->update_inputs(ns->tracker, timekeeping); 118 70 } ··· 125 77 int64_t *out_timestamp, 126 78 struct xrt_space_relation *out_relation) 127 79 { 128 - struct ns_hmd *ns = ns_hmd(xdev); 80 + struct ns_hmd *ns = get_ns_hmd(xdev); 81 + 129 82 130 83 // If the tracking device is created use it. 131 84 if (ns->tracker != NULL) { ··· 154 107 uint32_t view_index, 155 108 struct xrt_pose *out_pose) 156 109 { 157 - struct xrt_pose pose = {{0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 0.0f}}; 158 - bool adjust = view_index == 0; 159 - 160 - pose.position.x = eye_relation->x / 2.0f; 161 - pose.position.y = eye_relation->y / 2.0f; 162 - pose.position.z = eye_relation->z / 2.0f; 163 - 164 - // Adjust for left/right while also making sure there aren't any -0.f. 165 - if (pose.position.x > 0.0f && adjust) { 166 - pose.position.x = -pose.position.x; 167 - } 168 - if (pose.position.y > 0.0f && adjust) { 169 - pose.position.y = -pose.position.y; 170 - } 171 - if (pose.position.z > 0.0f && adjust) { 172 - pose.position.z = -pose.position.z; 173 - } 174 - 175 - *out_pose = pose; 110 + struct ns_hmd *ns = get_ns_hmd(xdev); 111 + *out_pose = ns->eye_configs[view_index].eye_pose; 176 112 } 177 113 178 114 ··· 183 119 */ 184 120 185 121 static void 186 - ns_mesh_calc(struct u_uv_generator *generator, 122 + ns_mesh_calc(struct u_uv_generator *gen, 187 123 int view, 188 124 float u, 189 125 float v, 190 126 struct u_uv_triplet *result) 191 127 { 192 - struct ns_mesh *mesh = (struct ns_mesh *)generator; 193 - (void)mesh; // Noop 128 + struct ns_mesh *mesh = ns_mesh(gen); 129 + 130 + struct ns_uv uv = {u, v}; 131 + struct ns_uv warped_uv = {0.0f, 0.0f}; 132 + ns_display_uv_to_render_uv(uv, &warped_uv, mesh->ns->eye_configs[view]); 194 133 195 - result->r.x = u; 196 - result->r.y = v; 197 - result->g.x = u; 198 - result->g.y = v; 199 - result->b.x = u; 200 - result->b.y = v; 134 + result->r.x = warped_uv.u; 135 + result->r.y = warped_uv.v; 136 + result->g.x = warped_uv.u; 137 + result->g.y = warped_uv.v; 138 + result->b.x = warped_uv.u; 139 + result->b.y = warped_uv.v; 201 140 } 202 141 203 142 static void 204 - ns_mesh_destroy(struct u_uv_generator *generator) 143 + ns_mesh_destroy(struct u_uv_generator *gen) 205 144 { 206 - struct ns_mesh *mesh = (struct ns_mesh *)generator; 145 + struct ns_mesh *mesh = (struct ns_mesh *)gen; 207 146 (void)mesh; // Noop 208 147 } 209 148 149 + static void 150 + ns_leap_parse(struct ns_leap *leap, struct cJSON *leap_data) 151 + { 152 + /* 153 + These are very wrong! 154 + You could very likely write into random memory here. 155 + 156 + u_json_get_string(cJSON_GetObjectItemCaseSensitive(leap_data, 157 + "name"), &leap->name); 158 + u_json_get_string(cJSON_GetObjectItemCaseSensitive(leap_data, 159 + "serial"), &leap->serial); 160 + */ 161 + 162 + u_json_get_vec3( 163 + cJSON_GetObjectItemCaseSensitive( 164 + cJSON_GetObjectItemCaseSensitive(leap_data, "localPose"), 165 + "position"), 166 + &leap->pose.position); 167 + u_json_get_quat( 168 + cJSON_GetObjectItemCaseSensitive( 169 + cJSON_GetObjectItemCaseSensitive(leap_data, "localPose"), 170 + "rotation"), 171 + &leap->pose.orientation); 172 + } 173 + 174 + static void 175 + ns_eye_parse(struct ns_eye *eye, struct cJSON *eye_data) 176 + { 177 + u_json_get_float( 178 + cJSON_GetObjectItemCaseSensitive(eye_data, "ellipseMinorAxis"), 179 + &eye->ellipse_minor_axis); 180 + u_json_get_float( 181 + cJSON_GetObjectItemCaseSensitive(eye_data, "ellipseMajorAxis"), 182 + &eye->ellipse_major_axis); 183 + u_json_get_vec3( 184 + cJSON_GetObjectItemCaseSensitive(eye_data, "screenForward"), 185 + &eye->screen_forward); 186 + u_json_get_vec3( 187 + cJSON_GetObjectItemCaseSensitive(eye_data, "screenPosition"), 188 + &eye->screen_position); 189 + u_json_get_vec3( 190 + cJSON_GetObjectItemCaseSensitive(eye_data, "eyePosition"), 191 + &eye->eye_pose.position); 192 + u_json_get_quat( 193 + cJSON_GetObjectItemCaseSensitive(eye_data, "eyeRotation"), 194 + &eye->eye_pose.orientation); 195 + u_json_get_quat( 196 + cJSON_GetObjectItemCaseSensitive(eye_data, "cameraProjection"), 197 + &eye->camera_projection); 198 + for (int x = 0; x < 4; ++x) { 199 + for (int y = 0; y < 4; ++y) { 200 + char key[4]; 201 + sprintf(key, "e%d%d", x, y); 202 + 203 + u_json_get_float( 204 + cJSON_GetObjectItemCaseSensitive( 205 + cJSON_GetObjectItemCaseSensitive( 206 + eye_data, "sphereToWorldSpace"), 207 + key), 208 + &eye->sphere_to_world_space.v[(x * 4) + y]); 209 + u_json_get_float( 210 + cJSON_GetObjectItemCaseSensitive( 211 + cJSON_GetObjectItemCaseSensitive( 212 + eye_data, "worldToScreenSpace"), 213 + key), 214 + &eye->world_to_screen_space.v[(x * 4) + y]); 215 + } 216 + } 217 + } 218 + 219 + 220 + /* 221 + * 222 + * Parse function. 223 + * 224 + */ 225 + 226 + static void 227 + ns_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection) 228 + { 229 + fov->angle_up = projection.x; // atanf(fabsf(projection.x) / 230 + // near_plane); 231 + fov->angle_down = 232 + projection.y; // atanf(fabsf(projection.y) / near_plane); 233 + fov->angle_left = 234 + projection.z; // atanf(fabsf(projection.z) / near_plane); 235 + fov->angle_right = 236 + projection.w; // atanf(fabsf(projection.w) / near_plane); 237 + } 238 + 239 + static bool 240 + ns_config_load(struct ns_hmd *ns) 241 + { 242 + // Get the path to the JSON file 243 + if (ns->config_path == NULL || strcmp(ns->config_path, "/") == 0) { 244 + NS_ERROR(ns, 245 + "Configuration path \"%s\" does not lead to a " 246 + "configuration JSON file. Set the NS_CONFIG_PATH env " 247 + "variable to your JSON.", 248 + ns->config_path); 249 + return false; 250 + } 251 + 252 + // Open the JSON file and put its contents into a string 253 + FILE *config_file = fopen(ns->config_path, "r"); 254 + if (config_file == NULL) { 255 + NS_ERROR( 256 + ns, 257 + "The configuration file at path \"%s\" was unable to load", 258 + ns->config_path); 259 + return false; 260 + } 261 + 262 + char json[8192]; 263 + size_t i = 0; 264 + while (!feof(config_file) && i < (sizeof(json) - 1)) { 265 + json[i++] = fgetc(config_file); 266 + } 267 + json[i] = '\0'; 268 + 269 + struct cJSON *config_json; 270 + 271 + // Parse the JSON file 272 + config_json = cJSON_Parse(json); 273 + if (config_json == NULL) { 274 + const char *error_ptr = cJSON_GetErrorPtr(); 275 + NS_ERROR(ns, "The JSON file at path \"%s\" was unable to parse", 276 + ns->config_path); 277 + if (error_ptr != NULL) { 278 + NS_ERROR(ns, "because of an error before %s", 279 + error_ptr); 280 + } 281 + return false; 282 + } 283 + 284 + ns_eye_parse(&ns->eye_configs[0], 285 + cJSON_GetObjectItemCaseSensitive(config_json, "leftEye")); 286 + ns_eye_parse(&ns->eye_configs[1], 287 + cJSON_GetObjectItemCaseSensitive(config_json, "rightEye")); 288 + ns_leap_parse(&ns->leap_config, cJSON_GetObjectItemCaseSensitive( 289 + config_json, "leapTracker")); 290 + cJSON_Delete(config_json); 291 + return true; 292 + } 293 + 210 294 211 295 /* 212 296 * ··· 226 310 ns->base.destroy = ns_hmd_destroy; 227 311 ns->base.name = XRT_DEVICE_GENERIC_HMD; 228 312 ns->pose.orientation.w = 1.0f; // All other values set to zero. 313 + ns->config_path = config_path; 229 314 ns->print_spew = print_spew; 230 315 ns->print_debug = print_debug; 231 316 ··· 237 322 238 323 // Setup info. 239 324 struct u_device_simple_info info; 240 - info.display.w_pixels = 1920; 241 - info.display.h_pixels = 1080; 325 + info.display.w_pixels = 2880; 326 + info.display.h_pixels = 1440; 242 327 info.display.w_meters = 0.13f; 243 328 info.display.h_meters = 0.07f; 244 329 info.lens_horizontal_separation_meters = 0.13f / 2.0f; 245 330 info.lens_vertical_position_meters = 0.07f / 2.0f; 246 - info.views[0].fov = 85.0f * (M_PI / 180.0f); 247 - info.views[1].fov = 85.0f * (M_PI / 180.0f); 331 + info.views[0].fov = 70.0f * (M_PI / 180.0f); 332 + info.views[1].fov = 70.0f * (M_PI / 180.0f); 333 + 334 + if (!ns_config_load(ns)) 335 + goto cleanup; 336 + 337 + ns_fov_calculate(&ns->base.hmd->views[0].fov, 338 + ns->eye_configs[0].camera_projection); 339 + ns_fov_calculate(&ns->base.hmd->views[1].fov, 340 + ns->eye_configs[1].camera_projection); 341 + 342 + // Create the optical systems 343 + ns->eye_configs[0].optical_system = 344 + ns_create_optical_system(&ns->eye_configs[0]); 345 + ns->eye_configs[1].optical_system = 346 + ns_create_optical_system(&ns->eye_configs[1]); 248 347 348 + // Setup the north star basic info 249 349 if (!u_device_setup_split_side_by_side(&ns->base, &info)) { 250 350 NS_ERROR(ns, "Failed to setup basic device info"); 251 - ns_hmd_destroy(&ns->base); 252 - return NULL; 351 + goto cleanup; 253 352 } 254 353 255 354 // If built, try to load the realsense tracker. ··· 264 363 // Setup the distortion mesh. 265 364 struct ns_mesh mesh; 266 365 U_ZERO(&mesh); 366 + mesh.ns = ns; 267 367 mesh.base.calc = ns_mesh_calc; 268 368 mesh.base.destroy = ns_mesh_destroy; 269 369 ··· 271 371 u_distortion_mesh_from_gen(&mesh.base, 2, ns->base.hmd); 272 372 273 373 return &ns->base; 374 + 375 + cleanup: 376 + ns_hmd_destroy(&ns->base); 377 + return NULL; 274 378 }

+196

src/xrt/drivers/north_star/ns_hmd.h

··· 1 + // Copyright 2019, Collabora, Ltd. 2 + // SPDX-License-Identifier: BSL-1.0 3 + /*! 4 + * @file 5 + * @brief Interface between North Star distortion and HMD code. 6 + * @author Nova King <technobaboo@gmail.com> 7 + * @ingroup drv_ns 8 + */ 9 + 10 + #pragma once 11 + 12 + #include "math/m_api.h" 13 + #include "util/u_distortion_mesh.h" 14 + #include "util/u_json.h" 15 + #include "util/u_misc.h" 16 + #include "xrt/xrt_defines.h" 17 + #include "xrt/xrt_device.h" 18 + 19 + #ifdef __cplusplus 20 + extern "C" { 21 + #endif 22 + 23 + /*! 24 + * @defgroup drv_ns North Star Driver 25 + * @ingroup drv 26 + * 27 + * @brief Driver for the North Star HMD. 28 + */ 29 + 30 + /* 31 + * 32 + * Defines 33 + * 34 + */ 35 + 36 + #define NS_SPEW(c, ...) \ 37 + do { \ 38 + if (c->print_spew) { \ 39 + fprintf(stderr, "%s - ", __func__); \ 40 + fprintf(stderr, __VA_ARGS__); \ 41 + fprintf(stderr, "\n"); \ 42 + } \ 43 + } while (false) 44 + 45 + #define NS_DEBUG(c, ...) \ 46 + do { \ 47 + if (c->print_debug) { \ 48 + fprintf(stderr, "%s - ", __func__); \ 49 + fprintf(stderr, __VA_ARGS__); \ 50 + fprintf(stderr, "\n"); \ 51 + } \ 52 + } while (false) 53 + 54 + #define NS_ERROR(c, ...) \ 55 + do { \ 56 + fprintf(stderr, "%s - ", __func__); \ 57 + fprintf(stderr, __VA_ARGS__); \ 58 + fprintf(stderr, "\n"); \ 59 + } while (false) 60 + 61 + 62 + /* 63 + * 64 + * Structs 65 + * 66 + */ 67 + 68 + 69 + /*! 70 + * Opaque struct for optical system C++ integration 71 + * 72 + * @ingroup drv_ns 73 + */ 74 + typedef struct _ns_optical_system ns_optical_system; 75 + 76 + 77 + /*! 78 + * Simple UV struct. 79 + * 80 + * @ingroup drv_ns 81 + */ 82 + struct ns_uv 83 + { 84 + float u; 85 + float v; 86 + }; 87 + 88 + 89 + /*! 90 + * Configuration information about the LMC or Rigel sensor according to the 91 + * configuration file. 92 + * 93 + * @ingroup drv_ns 94 + */ 95 + struct ns_leap 96 + { 97 + const char *name; 98 + const char *serial; 99 + struct xrt_pose pose; 100 + }; 101 + 102 + 103 + /*! 104 + * Distortion information about an eye parsed from the configuration file. 105 + * 106 + * @ingroup drv_ns 107 + */ 108 + struct ns_eye 109 + { 110 + float ellipse_minor_axis; 111 + float ellipse_major_axis; 112 + 113 + struct xrt_vec3 screen_forward; 114 + struct xrt_vec3 screen_position; 115 + 116 + struct xrt_pose eye_pose; 117 + 118 + struct xrt_quat camera_projection; 119 + 120 + struct xrt_matrix_4x4 sphere_to_world_space; 121 + struct xrt_matrix_4x4 world_to_screen_space; 122 + 123 + ns_optical_system *optical_system; 124 + }; 125 + 126 + /*! 127 + * Information about the whole North Star headset. 128 + * 129 + * @ingroup drv_ns 130 + */ 131 + struct ns_hmd 132 + { 133 + struct xrt_device base; 134 + struct xrt_pose pose; 135 + 136 + const char *config_path; 137 + 138 + struct ns_eye eye_configs[2]; 139 + struct ns_leap leap_config; 140 + 141 + struct xrt_device *tracker; 142 + 143 + bool print_spew; 144 + bool print_debug; 145 + }; 146 + 147 + /*! 148 + * The mesh generator for the North Star distortion. 149 + * 150 + * @ingroup drv_ns 151 + */ 152 + struct ns_mesh 153 + { 154 + struct u_uv_generator base; 155 + struct ns_hmd *ns; 156 + }; 157 + 158 + /*! 159 + * @dir drivers/north_star 160 + * 161 + * @brief @ref drv_ns files. 162 + */ 163 + 164 + 165 + /* 166 + * 167 + * Functions 168 + * 169 + */ 170 + 171 + /*! 172 + * Convert the display UV to the render UV using the distortion mesh. 173 + * 174 + * @ingroup drv_ns 175 + */ 176 + void 177 + ns_display_uv_to_render_uv(struct ns_uv display_uv, 178 + struct ns_uv *render_uv, 179 + struct ns_eye eye); 180 + 181 + /*! 182 + * Get the North Star HMD information from the xrt_device. 183 + * 184 + * @ingroup drv_ns 185 + */ 186 + struct ns_hmd * 187 + get_ns_hmd(struct xrt_device *xdev); 188 + 189 + 190 + ns_optical_system * 191 + ns_create_optical_system(struct ns_eye *eye); 192 + 193 + 194 + #ifdef __cplusplus 195 + } 196 + #endif

Configure Feed

Configure Feed