SoPickStyle和SoRayPickAction与SoPickedPoint

#include <stdlib.h>
#include <stdio.h>
#include <Inventor/SoDB.h>
#include <Inventor/nodes/SoPickStyle.h>
#include <Inventor/actions/SoRayPickAction.h>
#include <Inventor/SoPickedPoint.h>
#include <Inventor/SoPrimitiveVertex.h>
#include <Inventor/actions/SoCallbackAction.h>
#include <Inventor/actions/SoGetBoundingBoxAction.h>
#include <Inventor/nodes/SoCamera.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoFaceSet.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoNormal.h>
#include <Inventor/nodes/SoNormalBinding.h>
#include <Inventor/nodes/SoRotationXYZ.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoSwitch.h>
#include <Inventor/nodes/SoShape.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoSpotLight.h>
#include <Inventor/nodes/SoDirectionalLight.h>
#include <Inventor/nodes/SoPointLight.h>
#include <Inventor/nodes/SoText2.h>
#include <Inventor/nodes/SoText3.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/details/SoPointDetail.h>
#include <Inventor/details/SoFaceDetail.h>
#include <Inventor/nodes/SoIndexedTriangleStripSet.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoPerspectiveCamera.h>
#include <Inventor/nodes/SoTextureCoordinateBinding.h>
#include <Inventor/SbBox.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/Win/SoWin.h>
#include <Inventor/Win/viewers/SoWinExaminerViewer.h>

void pick(void* viewer,SoSensor* sensor);
void computeNewDirection(SbVec3f p, SbVec3f d, SbVec3f v);

float pos[10][3];
float dir[10][3];
float res[10][3];

SoWinExaminerViewer* viewer = NULL;

int main(int argc, char **argv)
{
	SoInput                 in;
	SoSeparator             *root;
	SoSeparator             *graph = NULL;

	printf("ARGV[0] %s\n",argv[0]);
	printf("ARGV[1] %s\n",argv[1]);

	HWND myWindow = SoWin::init("pick test");
	root = new SoSeparator;

	if ( argc > 1 ) 
	{
		if(!in.openFile(argv[1]))
		{
			fprintf(stderr, "%s: Cannot open %s\n", argv[0], argv[1]);
			exit(-1);
		}

		graph = SoDB::readAll(&in);

		if (graph == NULL)
		{
			fprintf(stderr, "%s: Problem reading data\n", argv[0]);
			exit(-1);
		}
		in.closeFile();
	}

	SoPickStyle* ps = new SoPickStyle();
	ps->style.setValue(SoPickStyle::SHAPE);

	root->addChild(ps);

	if (graph)
		root->addChild(graph);

	viewer = new SoWinExaminerViewer(myWindow);

	if ( argc == 1 )
	{
	<span style="white-space: pre;">	</span>SoCube* cube = new SoCube;
		cube->width.setValue(10000000);
		cube->height.setValue(10000000);
		cube->depth.setValue(1000);
		root->addChild(cube);
	}

	viewer->setSceneGraph(root);

	SoTimerSensor* timer = new SoTimerSensor(pick, root);

	timer->setInterval(1.0);
	timer->schedule();

	//init ray position info
	pos[0][0] = 5.57070325540446643e-10;
	pos[0][1] = -7.42488737159874290e-10;
	pos[0][2] = 1.03000000000000000e+05;

	dir[0][0] = 0;
	dir[0][1] = 0;
	dir[0][2] = -1;

	res[0][0] = 5.80091456384465921e-10;
	res[0][1] = -7.17818693374283612e-10;
	res[0][2] = 500;

	pos[1][0] = 5.57070325540446643e-10;
	pos[1][1] = -7.42488737159874290e-10;
	pos[1][2] = 1.03000000000000000e+05;

	dir[1][0] = -7.96414751318685998e-01;
	dir[1][1] = 5.07320812329469240e-01;
	dir[1][2] = -3.29164301313740471e-01;

	res[1][0] = -3.02754549589663453e+05;
	res[1][1] = 1.92856402747379179e+05;
	res[1][2] = 500;

	pos[2][0] = -5.33373295781723107e+05;
	pos[2][1] = -7.01097814155679225e+04;
	pos[2][2] = 1.62578282029601280e+05;

	dir[2][0] = -9.39640950030194710e-02;
	dir[2][1] = 8.63931765962725251e-01;
	dir[2][2] = -4.94765250003261992e-01;

	res[2][0] = -5.77151219564722967e+05;
	res[2][1] = 3.32396505723294045e+05;
	res[2][2] = 500;

	viewer->show();

	SoWin::show(myWindow);
	SoWin::mainLoop();
	//	pick(viewer,root);
}

void pick(void* data,SoSensor* sensor)
{
	static int num = 0;
	printf("In pick\n");
	SoRayPickAction pa(viewer->getViewportRegion());
	SbVec3f position(pos[num][0], pos[num][1], pos[num][2]);
	SbVec3f direction(dir[num][0], dir[num][1], dir[num][2]);
	pa.setRay(position, direction);
	pa.apply((SoNode*)data);
	const SoPickedPointList pList = pa.getPickedPointList();
	int numPicked = pList.getLength();
	if (numPicked > 0) 
	{
		printf("Found %d\n",numPicked);
		const SoPickedPoint* pp = pList[0];
		SbVec3f v = pp->getPoint();
		printf("PICKED POINT IS  %f %f %f\n",v[0],v[1],v[2]);
		printf("SHOULD HAVE BEEN %f %f %f\n",res[num][0],res[num][1],res[num][2]);
		computeNewDirection(position,direction,v);
	}

	if ( ++num > 2 ) 
		exit(0);
}

void computeNewDirection(SbVec3f p, SbVec3f d, SbVec3f v) 
{
	SbVec3f newD = -p + v;
	newD.normalize();
	double newDot = (double)newD.dot(d);
	if ( newDot > 1.0 ) 
		newDot = 1.0;

	double a = acos(newDot);
	printf("angle between new direction and original is: %f\n",a * ( 180/3.14 ) );
}