Найти мышиные мировые координаты (3D) на местности с квадрированным высотой

Я пытаюсь найти положение мыши в мировых координатах, но не могу найти правильный код. На данный момент я использую это, чтобы определить луч:

float pointX, pointY;
D3DXMATRIX projectionMatrix, viewMatrix, inverseViewMatrix, worldMatrix, translateMatrix, inverseWorldMatrix;
D3DXVECTOR3 direction, origin, rayOrigin, rayDirection;
bool intersect, result;// Move the mouse cursor coordinates into the -1 to +1 range.
pointX = ((2.0f * (float)mouseX) / (float)m_screenWidth) - 1.0f;
pointY = (((2.0f * (float)mouseY) / (float)m_screenHeight) - 1.0f) * -1.0f;

// Adjust the points using the projection matrix to account for the aspect ratio of the viewport.
m_Direct3D->GetProjectionMatrix(projectionMatrix);
pointX = pointX / projectionMatrix._11;
pointY = pointY / projectionMatrix._22;

// Get the inverse of the view matrix.
m_Camera->GetViewMatrix(viewMatrix);
D3DXMatrixInverse(&inverseViewMatrix, NULL, &viewMatrix);

// Calculate the direction of the picking ray in view space.
direction.x = (pointX * inverseViewMatrix._11) + (pointY * inverseViewMatrix._21) + inverseViewMatrix._31;
direction.y = (pointX * inverseViewMatrix._12) + (pointY * inverseViewMatrix._22) + inverseViewMatrix._32;
direction.z = (pointX * inverseViewMatrix._13) + (pointY * inverseViewMatrix._23) + inverseViewMatrix._33;

// Get the origin of the picking ray which is the position of the camera.
origin = m_Camera->GetPosition();

Это дает мне происхождение и направление луча.

Но…

Я использую пользовательскую сетку (не ту, что в DirectX) с картой высот, разделенную на квадродерево, и я не знаю, правильна ли моя логика, я попытался использовать усеченный конус, чтобы определить, какие узлы в квадродереве видны, и так же выполняется проверка пересечение треугольников только на этих узлах, вот этот код:

Примечание * m_mousepos — это вектор.

bool QuadTreeClass::getTriangleRay(NodeType* node, FrustumClass* frustum, ID3D10Device* device, D3DXVECTOR3 vPickRayDir, D3DXVECTOR3 vPickRayOrig){bool result;
int count, i, j, indexCount;
unsigned int stride, offset;
float fBary1, fBary2;
float fDist;
D3DXVECTOR3 v0, v1, v2;
float p1, p2, p3;// Check to see if the node can be viewed.
result = frustum->CheckCube(node->positionX, 0.0f, node->positionZ, (node->width / 2.0f));

if(!result)
{
return false;
}// If it can be seen then check all four child nodes to see if they can also be seen.
count = 0;
for(i=0; i<4; i++)
{
if(node->nodes[i] != 0)
{
count++;
getTriangleRay(node->nodes[i], frustum, device, vPickRayOrig, vPickRayDir);
}
}

// If there were any children nodes then dont continue

if(count != 0)
{
return false;
}

// Now intersect each triangle in this node

j = 0;

for(i=0; i<node->triangleCount; i++){

j = i * 3;

v0 = D3DXVECTOR3( node->vertexArray[j].x, node->vertexArray[j].y, node->vertexArray[j].z);
j++;
v1 = D3DXVECTOR3( node->vertexArray[j].x, node->vertexArray[j].y, node->vertexArray[j].z);
j++;
v2 = D3DXVECTOR3( node->vertexArray[j].x, node->vertexArray[j].y, node->vertexArray[j].z);

result = IntersectTriangle( vPickRayOrig, vPickRayDir, v0, v1, v2, &fDist, &fBary1, &fBary2);

if(result == true){

// intersection = true, so get a aproximate center of the triangle on the world

p1 = (v0.x + v0.x + v0.x)/3;
p2 = (v0.y + v1.y + v2.y)/3;
p3 = (v0.z + v1.z + v2.z)/3;

m_mousepos = D3DXVECTOR3(p1, p2, p3);

return true;

}

}}

bool QuadTreeClass::IntersectTriangle( const D3DXVECTOR3& orig, const D3DXVECTOR3& dir,D3DXVECTOR3& v0, D3DXVECTOR3& v1, D3DXVECTOR3& v2, FLOAT* t, FLOAT* u, FLOAT* v ){
// Find vectors for two edges sharing vert0
D3DXVECTOR3 edge1 = v1 - v0;
D3DXVECTOR3 edge2 = v2 - v0;

// Begin calculating determinant - also used to calculate U parameter
D3DXVECTOR3 pvec;
D3DXVec3Cross( &pvec, &dir, &edge2 );

// If determinant is near zero, ray lies in plane of triangle
FLOAT det = D3DXVec3Dot( &edge1, &pvec );

D3DXVECTOR3 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 = D3DXVec3Dot( &tvec, &pvec );
if( *u < 0.0f || *u > det )
return FALSE;

// Prepare to test V parameter
D3DXVECTOR3 qvec;
D3DXVec3Cross( &qvec, &tvec, &edge1 );

// Calculate V parameter and test bounds
*v = D3DXVec3Dot( &dir, &qvec );
if( *v < 0.0f || *u + *v > det )
return FALSE;

// Calculate t, scale parameters, ray intersects triangle
*t = D3DXVec3Dot( &edge2, &qvec );
FLOAT fInvDet = 1.0f / det;
*t *= fInvDet;
*u *= fInvDet;
*v *= fInvDet;return TRUE;
}

Пожалуйста, этот код правильный? Если это так, то моя проблема должна быть связана с квадри.

Спасибо!