Viewport cleanup (#233)

miniwin/src/d3drm/d3drmviewport.cpp

@@ -84,11 +84,8 @@ static void ComputeFrameWorldMatrix(IDirect3DRMFrame* frame, D3DRMMATRIX4D out)
 	IDirect3DRMFrame* cur = frame;
 	while (cur) {
 		auto* impl = static_cast<Direct3DRMFrameImpl*>(cur);
-		D3DRMMATRIX4D local;
-		memcpy(local, impl->m_transform, sizeof(local));
-
 		D3DRMMATRIX4D tmp;
-		D3DRMMatrixMultiply(tmp, local, acc);
+		D3DRMMatrixMultiply(tmp, impl->m_transform, acc);
 		memcpy(acc, tmp, sizeof(acc));
 
 		if (cur == impl->m_parent) {
@@ -99,22 +96,41 @@ static void ComputeFrameWorldMatrix(IDirect3DRMFrame* frame, D3DRMMATRIX4D out)
 	memcpy(out, acc, sizeof(acc));
 }
 
+inline D3DVECTOR CrossProduct(const D3DVECTOR& a, const D3DVECTOR& b)
+{
+	return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
+}
+
+inline D3DVECTOR Normalize(const D3DVECTOR& v)
+{
+	float len = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
+	if (len > 0.0f) {
+		float invLen = 1.0f / len;
+		return {v.x * invLen, v.y * invLen, v.z * invLen};
+	}
+	return {0, 0, 0};
+}
+
 D3DVECTOR ComputeTriangleNormal(const D3DVECTOR& v0, const D3DVECTOR& v1, const D3DVECTOR& v2)
 {
 	D3DVECTOR u = {v1.x - v0.x, v1.y - v0.y, v1.z - v0.z};
 	D3DVECTOR v = {v2.x - v0.x, v2.y - v0.y, v2.z - v0.z};
-	D3DVECTOR normal = {u.y * v.z - u.z * v.y, u.z * v.x - u.x * v.z, u.x * v.y - u.y * v.x};
+	D3DVECTOR normal = CrossProduct(u, v);
-	float len = std::sqrt(normal.x * normal.x + normal.y * normal.y + normal.z * normal.z);
+	normal = Normalize(normal);
-	if (len > 0.0f) {
-		normal.x /= len;
-		normal.y /= len;
-		normal.z /= len;
-	}
 
 	return normal;
 }
 
-inline D3DVECTOR TransformPoint(const D3DVECTOR& p, D3DRMMATRIX4D& m)
+inline D3DVECTOR TransformNormal(const D3DVECTOR& v, const Matrix3x3& m)
+{
+	return {
+		v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0],
+		v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1],
+		v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2]
+	};
+}
+
+inline D3DVECTOR TransformPoint(const D3DVECTOR& p, const D3DRMMATRIX4D& m)
 {
 	return {
 		p.x * m[0][0] + p.y * m[1][0] + p.z * m[2][0] + m[3][0],
@@ -125,13 +141,13 @@ inline D3DVECTOR TransformPoint(const D3DVECTOR& p, D3DRMMATRIX4D& m)
 
 void Direct3DRMViewportImpl::CollectLightsFromFrame(
 	IDirect3DRMFrame* frame,
-	D3DRMMATRIX4D parentMatrix,
+	D3DRMMATRIX4D parentToWorld,
 	std::vector<SceneLight>& lights
 )
 {
-	Direct3DRMFrameImpl* frameImpl = static_cast<Direct3DRMFrameImpl*>(frame);
+	auto* frameImpl = static_cast<Direct3DRMFrameImpl*>(frame);
 	D3DRMMATRIX4D worldMatrix;
-	D3DRMMatrixMultiply(worldMatrix, parentMatrix, frameImpl->m_transform);
+	D3DRMMatrixMultiply(worldMatrix, parentToWorld, frameImpl->m_transform);
 
 	IDirect3DRMLightArray* lightArray = nullptr;
 	frame->GetLights(&lightArray);
@@ -140,25 +156,21 @@ void Direct3DRMViewportImpl::CollectLightsFromFrame(
 		IDirect3DRMLight* light = nullptr;
 		lightArray->GetElement(li, &light);
 		D3DCOLOR color = light->GetColor();
-		D3DRMLIGHTTYPE type = light->GetType();
-
 		SceneLight extracted;
-		extracted.color.r = ((color >> 0) & 0xFF) / 255.0f;
+		extracted.color = {
-		extracted.color.g = ((color >> 8) & 0xFF) / 255.0f;
+			((color >> 0) & 0xFF) / 255.0f,
-		extracted.color.b = ((color >> 16) & 0xFF) / 255.0f;
+			((color >> 8) & 0xFF) / 255.0f,
-		extracted.color.a = ((color >> 24) & 0xFF) / 255.0f;
+			((color >> 16) & 0xFF) / 255.0f,
+			((color >> 24) & 0xFF) / 255.0f
+		};
 
+		D3DRMLIGHTTYPE type = light->GetType();
 		if (type == D3DRMLIGHT_POINT || type == D3DRMLIGHT_SPOT || type == D3DRMLIGHT_PARALLELPOINT) {
-			extracted.position.x = worldMatrix[3][0];
+			extracted.position = {worldMatrix[3][0], worldMatrix[3][1], worldMatrix[3][2]};
-			extracted.position.y = worldMatrix[3][1];
-			extracted.position.z = worldMatrix[3][2];
 			extracted.positional = 1.f;
 		}
-
 		if (type == D3DRMLIGHT_DIRECTIONAL || type == D3DRMLIGHT_SPOT) {
-			extracted.direction.x = worldMatrix[2][0];
+			extracted.direction = {worldMatrix[2][0], worldMatrix[2][1], worldMatrix[2][2]};
-			extracted.direction.y = worldMatrix[2][1];
-			extracted.direction.z = worldMatrix[2][2];
 			extracted.directional = 1.f;
 		}
 
@@ -189,10 +201,11 @@ void NormalizePlane(Plane& plane)
 	float len =
 		sqrtf(plane.normal.x * plane.normal.x + plane.normal.y * plane.normal.y + plane.normal.z * plane.normal.z);
 	if (len > 0.0f) {
-		plane.normal.x /= len;
+		float invLen = 1.0f / len;
-		plane.normal.y /= len;
+		plane.normal.x *= invLen;
-		plane.normal.z /= len;
+		plane.normal.y *= invLen;
-		plane.d /= len;
+		plane.normal.z *= invLen;
+		plane.d *= invLen;
 	}
 }
 
@@ -200,31 +213,12 @@ Plane frustumPlanes[6];
 
 void ExtractFrustumPlanes(const D3DRMMATRIX4D& m)
 {
-	// Left
+	static const int idx[][2] = {{0, 1}, {0, -1}, {1, 1}, {1, -1}, {2, 1}, {2, -1}};
-	frustumPlanes[0].normal = {m[0][3] + m[0][0], m[1][3] + m[1][0], m[2][3] + m[2][0]};
-	frustumPlanes[0].d = m[3][3] + m[3][0];
-
-	// Right
-	frustumPlanes[1].normal = {m[0][3] - m[0][0], m[1][3] - m[1][0], m[2][3] - m[2][0]};
-	frustumPlanes[1].d = m[3][3] - m[3][0];
-
-	// Bottom
-	frustumPlanes[2].normal = {m[0][3] + m[0][1], m[1][3] + m[1][1], m[2][3] + m[2][1]};
-	frustumPlanes[2].d = m[3][3] + m[3][1];
-
-	// Top
-	frustumPlanes[3].normal = {m[0][3] - m[0][1], m[1][3] - m[1][1], m[2][3] - m[2][1]};
-	frustumPlanes[3].d = m[3][3] - m[3][1];
-
-	// Near
-	frustumPlanes[4].normal = {m[0][3] + m[0][2], m[1][3] + m[1][2], m[2][3] + m[2][2]};
-	frustumPlanes[4].d = m[3][3] + m[3][2];
-
-	// Far
-	frustumPlanes[5].normal = {m[0][3] - m[0][2], m[1][3] - m[1][2], m[2][3] - m[2][2]};
-	frustumPlanes[5].d = m[3][3] - m[3][2];
-
 	for (int i = 0; i < 6; ++i) {
+		int axis = idx[i][0], sign = idx[i][1];
+		frustumPlanes[i]
+			.normal = {m[0][3] + sign * m[0][axis], m[1][3] + sign * m[1][axis], m[2][3] + sign * m[2][axis]};
+		frustumPlanes[i].d = m[3][3] + sign * m[3][axis];
 		NormalizePlane(frustumPlanes[i]);
 	}
 }
@@ -234,15 +228,14 @@ bool IsBoxInFrustum(const D3DVECTOR corners[8], const Plane planes[6])
 	for (int i = 0; i < 6; ++i) {
 		int out = 0;
 		for (int j = 0; j < 8; ++j) {
-			const D3DVECTOR& pt = corners[j];
+			float dist = planes[i].normal.x * corners[j].x + planes[i].normal.y * corners[j].y +
-			float dist =
+						 planes[i].normal.z * corners[j].z + planes[i].d;
-				planes[i].normal.x * pt.x + planes[i].normal.y * pt.y + planes[i].normal.z * pt.z + planes[i].d;
 			if (dist < 0.0f) {
 				++out;
 			}
 		}
 		if (out == 8) {
-			return false; // All corners are outside this plane
+			return false;
 		}
 	}
 	return true;
@@ -259,8 +252,9 @@ void Direct3DRMViewportImpl::CollectMeshesFromFrame(
 	memcpy(localMatrix, frameImpl->m_transform, sizeof(D3DRMMATRIX4D));
 
 	D3DRMMATRIX4D worldMatrix;
-	Matrix3x3 worldMatrixInvert;
 	D3DRMMatrixMultiply(worldMatrix, parentMatrix, localMatrix);
+
+	Matrix3x3 worldMatrixInvert;
 	D3DRMMatrixInvertForNormal(worldMatrixInvert, worldMatrix);
 
 	IDirect3DRMVisualArray* visuals = nullptr;
@@ -281,121 +275,95 @@ void Direct3DRMViewportImpl::CollectMeshesFromFrame(
 
 		IDirect3DRMMesh* mesh = nullptr;
 		visual->QueryInterface(IID_IDirect3DRMMesh, (void**) &mesh);
-		if (mesh) {
+		if (!mesh) {
-			D3DRMBOX box;
+			visual->Release();
-			mesh->GetBox(&box);
+			continue;
-			D3DVECTOR boxCorners[8] = {
-				{box.min.x, box.min.y, box.min.z},
-				{box.min.x, box.min.y, box.max.z},
-				{box.min.x, box.max.y, box.min.z},
-				{box.min.x, box.max.y, box.max.z},
-				{box.max.x, box.min.y, box.min.z},
-				{box.max.x, box.min.y, box.max.z},
-				{box.max.x, box.max.y, box.min.z},
-				{box.max.x, box.max.y, box.max.z},
-			};
-			for (D3DVECTOR& boxCorner : boxCorners) {
-				boxCorner = TransformPoint(boxCorner, worldMatrix);
-			}
-			if (!IsBoxInFrustum(boxCorners, frustumPlanes)) {
-				mesh->Release();
-				visual->Release();
-				continue;
-			}
-
-			DWORD groupCount = mesh->GetGroupCount();
-			for (DWORD gi = 0; gi < groupCount; ++gi) {
-				DWORD vtxCount, faceCount, vpf, dataSize;
-				mesh->GetGroup(gi, &vtxCount, &faceCount, &vpf, &dataSize, nullptr);
-
-				std::vector<D3DRMVERTEX> d3dVerts(vtxCount);
-				std::vector<DWORD> faces(dataSize);
-				mesh->GetVertices(gi, 0, vtxCount, d3dVerts.data());
-				mesh->GetGroup(gi, nullptr, nullptr, nullptr, nullptr, faces.data());
-
-				D3DCOLOR color = mesh->GetGroupColor(gi);
-				D3DRMRENDERQUALITY quality = mesh->GetGroupQuality(gi);
-				IDirect3DRMTexture* texture = nullptr;
-				mesh->GetGroupTexture(gi, &texture);
-				IDirect3DRMMaterial* material = nullptr;
-				mesh->GetGroupMaterial(gi, &material);
-				Uint32 texId = NO_TEXTURE_ID;
-				if (texture) {
-					texId = m_renderer->GetTextureId(texture);
-					texture->Release();
-				}
-				float shininess = 0.0f;
-				if (material) {
-					shininess = material->GetPower();
-					material->Release();
-				}
-
-				for (DWORD fi = 0; fi < faceCount; ++fi) {
-					D3DVECTOR norm;
-					if (quality == D3DRMRENDER_FLAT || quality == D3DRMRENDER_UNLITFLAT) {
-						D3DRMVERTEX& v0 = d3dVerts[faces[fi * vpf + 0]];
-						D3DRMVERTEX& v1 = d3dVerts[faces[fi * vpf + 1]];
-						D3DRMVERTEX& v2 = d3dVerts[faces[fi * vpf + 2]];
-						norm = ComputeTriangleNormal(v0.position, v1.position, v2.position);
-					}
-
-					for (int idx = 0; idx < vpf; ++idx) {
-						D3DRMVERTEX& dv = d3dVerts[faces[fi * vpf + idx]];
-						D3DVECTOR pos = dv.position;
-						if (quality == D3DRMRENDER_GOURAUD || quality == D3DRMRENDER_PHONG) {
-							norm = dv.normal;
-						}
-
-						D3DVECTOR worldPos;
-						worldPos.x = pos.x * worldMatrix[0][0] + pos.y * worldMatrix[1][0] + pos.z * worldMatrix[2][0] +
-									 worldMatrix[3][0];
-						worldPos.y = pos.x * worldMatrix[0][1] + pos.y * worldMatrix[1][1] + pos.z * worldMatrix[2][1] +
-									 worldMatrix[3][1];
-						worldPos.z = pos.x * worldMatrix[0][2] + pos.y * worldMatrix[1][2] + pos.z * worldMatrix[2][2] +
-									 worldMatrix[3][2];
-
-						D3DVECTOR viewPos;
-						viewPos.x = worldPos.x * m_viewMatrix[0][0] + worldPos.y * m_viewMatrix[1][0] +
-									worldPos.z * m_viewMatrix[2][0] + m_viewMatrix[3][0];
-						viewPos.y = worldPos.x * m_viewMatrix[0][1] + worldPos.y * m_viewMatrix[1][1] +
-									worldPos.z * m_viewMatrix[2][1] + m_viewMatrix[3][1];
-						viewPos.z = worldPos.x * m_viewMatrix[0][2] + worldPos.y * m_viewMatrix[1][2] +
-									worldPos.z * m_viewMatrix[2][2] + m_viewMatrix[3][2];
-
-						D3DVECTOR viewNorm;
-						viewNorm.x = norm.x * worldMatrixInvert[0][0] + norm.y * worldMatrixInvert[1][0] +
-									 norm.z * worldMatrixInvert[2][0];
-						viewNorm.y = norm.x * worldMatrixInvert[0][1] + norm.y * worldMatrixInvert[1][1] +
-									 norm.z * worldMatrixInvert[2][1];
-						viewNorm.z = norm.x * worldMatrixInvert[0][2] + norm.y * worldMatrixInvert[1][2] +
-									 norm.z * worldMatrixInvert[2][2];
-
-						float len = sqrtf(viewNorm.x * viewNorm.x + viewNorm.y * viewNorm.y + viewNorm.z * viewNorm.z);
-						if (len > 0.0f) {
-							float invLen = 1.0f / len;
-							viewNorm.x *= invLen;
-							viewNorm.y *= invLen;
-							viewNorm.z *= invLen;
-						}
-
-						PositionColorVertex vtx;
-						vtx.position = viewPos;
-						vtx.normals = viewNorm;
-						vtx.colors = {
-							static_cast<Uint8>((color >> 16) & 0xFF),
-							static_cast<Uint8>((color >> 8) & 0xFF),
-							static_cast<Uint8>((color >> 0) & 0xFF),
-							static_cast<Uint8>((color >> 24) & 0xFF)
-						};
-						vtx.shininess = shininess;
-						vtx.texId = texId;
-						vtx.texCoord = {dv.tu, dv.tv};
-						verts.push_back(vtx);
-					}
-				}
-			}
-			mesh->Release();
 		}
+
+		D3DRMBOX box;
+		mesh->GetBox(&box);
+		D3DVECTOR boxCorners[8] = {
+			{box.min.x, box.min.y, box.min.z},
+			{box.min.x, box.min.y, box.max.z},
+			{box.min.x, box.max.y, box.min.z},
+			{box.min.x, box.max.y, box.max.z},
+			{box.max.x, box.min.y, box.min.z},
+			{box.max.x, box.min.y, box.max.z},
+			{box.max.x, box.max.y, box.min.z},
+			{box.max.x, box.max.y, box.max.z},
+		};
+		for (D3DVECTOR& boxCorner : boxCorners) {
+			boxCorner = TransformPoint(boxCorner, worldMatrix);
+		}
+		if (!IsBoxInFrustum(boxCorners, frustumPlanes)) {
+			mesh->Release();
+			visual->Release();
+			continue;
+		}
+
+		DWORD groupCount = mesh->GetGroupCount();
+		for (DWORD gi = 0; gi < groupCount; ++gi) {
+			DWORD vtxCount, faceCount, vpf, dataSize;
+			mesh->GetGroup(gi, &vtxCount, &faceCount, &vpf, &dataSize, nullptr);
+
+			std::vector<D3DRMVERTEX> d3dVerts(vtxCount);
+			std::vector<DWORD> faces(dataSize);
+			mesh->GetVertices(gi, 0, vtxCount, d3dVerts.data());
+			mesh->GetGroup(gi, nullptr, nullptr, nullptr, nullptr, faces.data());
+
+			D3DCOLOR color = mesh->GetGroupColor(gi);
+			D3DRMRENDERQUALITY quality = mesh->GetGroupQuality(gi);
+
+			IDirect3DRMTexture* texture = nullptr;
+			mesh->GetGroupTexture(gi, &texture);
+			Uint32 texId = NO_TEXTURE_ID;
+			if (texture) {
+				texId = m_renderer->GetTextureId(texture);
+				texture->Release();
+			}
+
+			IDirect3DRMMaterial* material = nullptr;
+			mesh->GetGroupMaterial(gi, &material);
+			float shininess = 0.0f;
+			if (material) {
+				shininess = material->GetPower();
+				material->Release();
+			}
+
+			for (DWORD fi = 0; fi < faceCount; ++fi) {
+				D3DVECTOR norm;
+				if (quality == D3DRMRENDER_FLAT || quality == D3DRMRENDER_UNLITFLAT) {
+					D3DRMVERTEX& v0 = d3dVerts[faces[fi * vpf + 0]];
+					D3DRMVERTEX& v1 = d3dVerts[faces[fi * vpf + 1]];
+					D3DRMVERTEX& v2 = d3dVerts[faces[fi * vpf + 2]];
+					norm = ComputeTriangleNormal(v0.position, v1.position, v2.position);
+				}
+
+				for (DWORD idx = 0; idx < vpf; ++idx) {
+					D3DRMVERTEX& dv = d3dVerts[faces[fi * vpf + idx]];
+					D3DVECTOR pos = dv.position;
+					if (quality == D3DRMRENDER_GOURAUD || quality == D3DRMRENDER_PHONG) {
+						norm = dv.normal;
+					}
+
+					D3DVECTOR worldPos = TransformPoint(pos, worldMatrix);
+					D3DVECTOR viewNorm = TransformNormal(norm, worldMatrixInvert);
+
+					verts.push_back(
+						{TransformPoint(worldPos, m_viewMatrix),
+						 Normalize(viewNorm),
+						 {static_cast<Uint8>((color >> 16) & 0xFF),
+						  static_cast<Uint8>((color >> 8) & 0xFF),
+						  static_cast<Uint8>((color >> 0) & 0xFF),
+						  static_cast<Uint8>((color >> 24) & 0xFF)},
+						 texId,
+						 {dv.tu, dv.tv},
+						 shininess}
+					);
+				}
+			}
+		}
+		mesh->Release();
 		visual->Release();
 	}
 	visuals->Release();
@@ -405,7 +373,7 @@ void Direct3DRMViewportImpl::CollectSceneData()
 {
 	m_backgroundColor = static_cast<Direct3DRMFrameImpl*>(m_rootFrame)->m_backgroundColor;
 
-	// Compute camera matrix
+	// Compute view-projection matrix
 	D3DRMMATRIX4D cameraWorld, viewProj;
 	ComputeFrameWorldMatrix(m_camera, cameraWorld);
 	D3DRMMatrixInvertOrthogonal(m_viewMatrix, cameraWorld);
@@ -450,7 +418,8 @@ HRESULT Direct3DRMViewportImpl::Clear()
 	uint8_t b = m_backgroundColor & 0xFF;
 
 	Uint32 color = SDL_MapRGB(SDL_GetPixelFormatDetails(DDBackBuffer->format), nullptr, r, g, b);
-	SDL_FillSurfaceRect(DDBackBuffer, NULL, color);
+	SDL_FillSurfaceRect(DDBackBuffer, nullptr, color);
+
 	return DD_OK;
 }
 
@@ -556,39 +525,39 @@ DWORD Direct3DRMViewportImpl::GetHeight()
 	return m_height;
 }
 
+inline float FromNDC(float ndcCoord, float dim)
+{
+	return (ndcCoord * 0.5f + 0.5f) * dim;
+}
+
+inline void MultiplyMatrixVec4(D3DRMVECTOR4D& out, const D3DRMMATRIX4D& mat, const D3DRMVECTOR4D& vec)
+{
+	out.x = mat[0][0] * vec.x + mat[1][0] * vec.y + mat[2][0] * vec.z + mat[3][0] * vec.w;
+	out.y = mat[0][1] * vec.x + mat[1][1] * vec.y + mat[2][1] * vec.z + mat[3][1] * vec.w;
+	out.z = mat[0][2] * vec.x + mat[1][2] * vec.y + mat[2][2] * vec.z + mat[3][2] * vec.w;
+	out.w = mat[0][3] * vec.x + mat[1][3] * vec.y + mat[2][3] * vec.z + mat[3][3] * vec.w;
+}
+
 HRESULT Direct3DRMViewportImpl::Transform(D3DRMVECTOR4D* screen, D3DVECTOR* world)
 {
 	D3DRMVECTOR4D worldVec = {world->x, world->y, world->z, 1.0f};
+	D3DRMVECTOR4D viewVec, projVec;
 
-	D3DRMVECTOR4D viewVec;
+	MultiplyMatrixVec4(viewVec, m_viewMatrix, worldVec);
-	viewVec.x = m_viewMatrix[0][0] * worldVec.x + m_viewMatrix[1][0] * worldVec.y + m_viewMatrix[2][0] * worldVec.z +
+	MultiplyMatrixVec4(projVec, m_projectionMatrix, viewVec);
-				m_viewMatrix[3][0] * worldVec.w;
-	viewVec.y = m_viewMatrix[0][1] * worldVec.x + m_viewMatrix[1][1] * worldVec.y + m_viewMatrix[2][1] * worldVec.z +
-				m_viewMatrix[3][1] * worldVec.w;
-	viewVec.z = m_viewMatrix[0][2] * worldVec.x + m_viewMatrix[1][2] * worldVec.y + m_viewMatrix[2][2] * worldVec.z +
-				m_viewMatrix[3][2] * worldVec.w;
-	viewVec.w = m_viewMatrix[0][3] * worldVec.x + m_viewMatrix[1][3] * worldVec.y + m_viewMatrix[2][3] * worldVec.z +
-				m_viewMatrix[3][3] * worldVec.w;
 
-	screen->x = viewVec.x * m_projectionMatrix[0][0] + viewVec.y * m_projectionMatrix[1][0] +
+	*screen = projVec;
-				viewVec.z * m_projectionMatrix[2][0] + viewVec.w * m_projectionMatrix[3][0];
-	screen->y = viewVec.x * m_projectionMatrix[0][1] + viewVec.y * m_projectionMatrix[1][1] +
-				viewVec.z * m_projectionMatrix[2][1] + viewVec.w * m_projectionMatrix[3][1];
-	screen->z = viewVec.x * m_projectionMatrix[0][2] + viewVec.y * m_projectionMatrix[1][2] +
-				viewVec.z * m_projectionMatrix[2][2] + viewVec.w * m_projectionMatrix[3][2];
-	screen->w = viewVec.x * m_projectionMatrix[0][3] + viewVec.y * m_projectionMatrix[1][3] +
-				viewVec.z * m_projectionMatrix[2][3] + viewVec.w * m_projectionMatrix[3][3];
 
-	float invW = 1.0f / screen->w;
+	float invW = 1.0f / projVec.w;
-	float ndcX = screen->x * invW;
+	float ndcX = projVec.x * invW;
-	float ndcY = screen->y * invW;
+	float ndcY = projVec.y * invW;
 
-	screen->x = (ndcX * 0.5f + 0.5f) * m_width;
+	screen->x = FromNDC(ndcX, m_width);
-	screen->y = (1.0f - (ndcY * 0.5f + 0.5f)) * m_height;
+	screen->y = FromNDC(-ndcY, m_height); // Y-flip
 
-	// Undo perspective divide
+	// Undo perspective divide for screen-space coords
-	screen->x *= screen->z;
+	screen->x *= projVec.z;
-	screen->y *= screen->w;
+	screen->y *= projVec.w;
 
 	return DD_OK;
 }
@@ -603,35 +572,27 @@ HRESULT Direct3DRMViewportImpl::InverseTransform(D3DVECTOR* world, D3DRMVECTOR4D
 	float ndcX = screenX / m_width * 2.0f - 1.0f;
 	float ndcY = 1.0f - (screenY / m_height) * 2.0f;
 
-	float clipVec[4] = {ndcX * screen->w, ndcY * screen->w, screen->z, screen->w};
+	D3DRMVECTOR4D clipVec = {ndcX * screen->w, ndcY * screen->w, screen->z, screen->w};
 
-	float viewVec[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+	D3DRMVECTOR4D viewVec;
-	for (int i = 0; i < 4; i++) {
+	MultiplyMatrixVec4(viewVec, m_inverseProjectionMatrix, clipVec);
-		for (int j = 0; j < 4; j++) {
-			viewVec[j] += m_inverseProjectionMatrix[i][j] * clipVec[i];
-		}
-	}
 
 	D3DRMMATRIX4D inverseViewMatrix;
 	D3DRMMatrixInvertOrthogonal(inverseViewMatrix, m_viewMatrix);
 
-	float worldVec[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+	D3DRMVECTOR4D worldVec;
-	for (int i = 0; i < 4; i++) {
+	MultiplyMatrixVec4(worldVec, inverseViewMatrix, viewVec);
-		for (int j = 0; j < 4; j++) {
-			worldVec[j] += inverseViewMatrix[i][j] * viewVec[i];
-		}
-	}
 
 	// Perspective divide
-	if (worldVec[3] != 0.0f) {
+	if (worldVec.w != 0.0f) {
-		world->x = worldVec[0] / worldVec[3];
+		world->x = worldVec.x / worldVec.w;
-		world->y = worldVec[1] / worldVec[3];
+		world->y = worldVec.y / worldVec.w;
-		world->z = worldVec[2] / worldVec[3];
+		world->z = worldVec.z / worldVec.w;
 	}
 	else {
-		world->x = worldVec[0];
+		world->x = worldVec.x;
-		world->y = worldVec[1];
+		world->y = worldVec.y;
-		world->z = worldVec[2];
+		world->z = worldVec.z;
 	}
 
 	return DD_OK;
@@ -713,9 +674,7 @@ Ray BuildPickingRay(
 
 	// Normalize ray direction
 	float len = sqrt(DotProduct(rayDirView, rayDirView));
-	rayDirView.x /= len;
+	rayDirView = Normalize(rayDirView);
-	rayDirView.y /= len;
-	rayDirView.z /= len;
 
 	// Compute camera world matrix and invert it to get view->world
 	D3DRMMATRIX4D cameraWorld;
@@ -731,18 +690,11 @@ Ray BuildPickingRay(
 	};
 
 	len = sqrt(rayDirWorld.x * rayDirWorld.x + rayDirWorld.y * rayDirWorld.y + rayDirWorld.z * rayDirWorld.z);
-	rayDirWorld.x /= len;
+	rayDirWorld = Normalize(rayDirWorld);
-	rayDirWorld.y /= len;
-	rayDirWorld.z /= len;
 
 	return Ray{rayOriginWorld, rayDirWorld};
 }
 
-inline D3DVECTOR CrossProduct(const D3DVECTOR& a, const D3DVECTOR& b)
-{
-	return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
-}
-
 bool RayIntersectsTriangle(
 	const Ray& ray,
 	const D3DVECTOR& v0,
@@ -813,12 +765,7 @@ bool RayIntersectsMeshTriangles(
 			D3DVECTOR tri[3];
 			for (int j = 0; j < 3; ++j) {
 				const D3DVECTOR& v = vertices[(j == 0 ? i0 : (j == 1 ? i1 : i2))].position;
-				tri[j].x =
+				tri[j] = TransformPoint(v, worldMatrix);
-					v.x * worldMatrix[0][0] + v.y * worldMatrix[1][0] + v.z * worldMatrix[2][0] + worldMatrix[3][0];
-				tri[j].y =
-					v.x * worldMatrix[0][1] + v.y * worldMatrix[1][1] + v.z * worldMatrix[2][1] + worldMatrix[3][1];
-				tri[j].z =
-					v.x * worldMatrix[0][2] + v.y * worldMatrix[1][2] + v.z * worldMatrix[2][2] + worldMatrix[3][2];
 			}
 
 			float dist;
@@ -833,6 +780,43 @@ bool RayIntersectsMeshTriangles(
 	return false;
 }
 
+inline D3DVECTOR TransformVector(const D3DRMMATRIX4D& mat, const D3DVECTOR& vec)
+{
+	return {
+		vec.x * mat[0][0] + vec.y * mat[1][0] + vec.z * mat[2][0] + mat[3][0],
+		vec.x * mat[0][1] + vec.y * mat[1][1] + vec.z * mat[2][1] + mat[3][1],
+		vec.x * mat[0][2] + vec.y * mat[1][2] + vec.z * mat[2][2] + mat[3][2]
+	};
+}
+
+D3DRMBOX ComputeTransformedAABB(const D3DRMBOX& box, const D3DRMMATRIX4D& mat)
+{
+	D3DVECTOR corners[8] = {
+		{box.min.x, box.min.y, box.min.z},
+		{box.min.x, box.min.y, box.max.z},
+		{box.min.x, box.max.y, box.min.z},
+		{box.min.x, box.max.y, box.max.z},
+		{box.max.x, box.min.y, box.min.z},
+		{box.max.x, box.min.y, box.max.z},
+		{box.max.x, box.max.y, box.min.z},
+		{box.max.x, box.max.y, box.max.z}
+	};
+
+	D3DVECTOR transformed = TransformVector(mat, corners[0]);
+	D3DRMBOX worldBox = {transformed, transformed};
+
+	for (int i = 1; i < 8; ++i) {
+		D3DVECTOR v = TransformVector(mat, corners[i]);
+		worldBox.min.x = std::min(worldBox.min.x, v.x);
+		worldBox.min.y = std::min(worldBox.min.y, v.y);
+		worldBox.min.z = std::min(worldBox.min.z, v.z);
+		worldBox.max.x = std::max(worldBox.max.x, v.x);
+		worldBox.max.y = std::max(worldBox.max.y, v.y);
+		worldBox.max.z = std::max(worldBox.max.z, v.z);
+	}
+	return worldBox;
+}
+
 HRESULT Direct3DRMViewportImpl::Pick(float x, float y, LPDIRECT3DRMPICKEDARRAY* pickedArray)
 {
 	if (!m_rootFrame) {
@@ -855,8 +839,7 @@ HRESULT Direct3DRMViewportImpl::Pick(float x, float y, LPDIRECT3DRMPICKEDARRAY*
 
 	std::function<void(IDirect3DRMFrame*, std::vector<IDirect3DRMFrame*>&)> recurse;
 	recurse = [&](IDirect3DRMFrame* frame, std::vector<IDirect3DRMFrame*>& path) {
-		Direct3DRMFrameImpl* frameImpl = static_cast<Direct3DRMFrameImpl*>(frame);
+		path.push_back(frame);
-		path.push_back(frame); // Push current frame
 
 		IDirect3DRMVisualArray* visuals = nullptr;
 		frame->GetVisuals(&visuals);
@@ -882,63 +865,9 @@ HRESULT Direct3DRMViewportImpl::Pick(float x, float y, LPDIRECT3DRMPICKEDARRAY*
 				// Transform box corners to world space
 				D3DRMMATRIX4D worldMatrix;
 				ComputeFrameWorldMatrix(frame, worldMatrix);
+				D3DRMBOX worldBox = ComputeTransformedAABB(box, worldMatrix);
 
-				// Transform box min and max points
+				float distance = FLT_MAX;
-				// Because axis-aligned box can become oriented box after transform,
-				// but we simplify by transforming all 8 corners and computing new AABB
-
-				D3DVECTOR corners[8] = {
-					{box.min.x, box.min.y, box.min.z},
-					{box.min.x, box.min.y, box.max.z},
-					{box.min.x, box.max.y, box.min.z},
-					{box.min.x, box.max.y, box.max.z},
-					{box.max.x, box.min.y, box.min.z},
-					{box.max.x, box.min.y, box.max.z},
-					{box.max.x, box.max.y, box.min.z},
-					{box.max.x, box.max.y, box.max.z},
-				};
-
-				D3DRMBOX worldBox;
-				{
-					float x = corners[0].x * worldMatrix[0][0] + corners[0].y * worldMatrix[1][0] +
-							  corners[0].z * worldMatrix[2][0] + worldMatrix[3][0];
-					float y = corners[0].x * worldMatrix[0][1] + corners[0].y * worldMatrix[1][1] +
-							  corners[0].z * worldMatrix[2][1] + worldMatrix[3][1];
-					float z = corners[0].x * worldMatrix[0][2] + corners[0].y * worldMatrix[1][2] +
-							  corners[0].z * worldMatrix[2][2] + worldMatrix[3][2];
-					worldBox.min = {x, y, z};
-					worldBox.max = {x, y, z};
-				}
-
-				for (int c = 1; c < 8; ++c) {
-					float x = corners[c].x * worldMatrix[0][0] + corners[c].y * worldMatrix[1][0] +
-							  corners[c].z * worldMatrix[2][0] + worldMatrix[3][0];
-					float y = corners[c].x * worldMatrix[0][1] + corners[c].y * worldMatrix[1][1] +
-							  corners[c].z * worldMatrix[2][1] + worldMatrix[3][1];
-					float z = corners[c].x * worldMatrix[0][2] + corners[c].y * worldMatrix[1][2] +
-							  corners[c].z * worldMatrix[2][2] + worldMatrix[3][2];
-
-					if (x < worldBox.min.x) {
-						worldBox.min.x = x;
-					}
-					if (y < worldBox.min.y) {
-						worldBox.min.y = y;
-					}
-					if (z < worldBox.min.z) {
-						worldBox.min.z = z;
-					}
-					if (x > worldBox.max.x) {
-						worldBox.max.x = x;
-					}
-					if (y > worldBox.max.y) {
-						worldBox.max.y = y;
-					}
-					if (z > worldBox.max.z) {
-						worldBox.max.z = z;
-					}
-				}
-
-				float distance = 0.0f;
 				if (RayIntersectsBox(pickRay, worldBox, distance) &&
 					RayIntersectsMeshTriangles(pickRay, mesh, worldMatrix, distance)) {
 					auto* arr = new Direct3DRMFrameArrayImpl();
@@ -946,10 +875,7 @@ HRESULT Direct3DRMViewportImpl::Pick(float x, float y, LPDIRECT3DRMPICKEDARRAY*
 						arr->AddElement(f);
 					}
 
-					PickRecord rec;
+					PickRecord rec = {visual, arr, {distance}};
-					rec.visual = visual;
-					rec.frameArray = arr;
-					rec.desc.dist = distance;
 					hits.push_back(rec);
 				}
 				mesh->Release();