点到平面距离的推导公式
d取绝对值前为正则Q点在平面同侧,反之异侧。
根据求导公式就很容易得到代码如下:
// Calculate the normal distance to plane from cameraPose, the given planePose should have y axis
// parallel to plane's normal, for example plane's center pose or hit test pose.
private static float calculateDistanceToPlane(Pose planePose, Pose cameraPose) {
float[] normal = new float[3];
float cameraX = cameraPose.tx();
float cameraY = cameraPose.ty();
float cameraZ = cameraPose.tz();
// Get transformed Y axis of plane's coordinate system.
planePose.getTransformedAxis(1, 1.0f, normal, 0);
// Compute dot product of plane's normal with vector from camera to plane center.
return (cameraX - planePose.tx()) * normal[0]
+ (cameraY - planePose.ty()) * normal[1]
+ (cameraZ - planePose.tz()) * normal[2];
}