IMP_S32 ipInOscRegion(IpTrackedTarget *pstTarget,POLYGON_REGION_S stOscRegion)
{
Point apolygon[10], point;
IMP_S32 dwPI;
IMP_S32 s32PntNum = 0;
IMP_S32 s32Ret = 0;
IpTargetPosition *pstPos = ipTargetTrajectoryGetPosition( &pstTarget->stTrajectory, 0 );
IMP_S32 s32Count=0; //交点数
s32PntNum = stOscRegion.s32PointNum;
for (dwPI = 0; dwPI < s32PntNum; dwPI++)
{
apolygon[dwPI].x = stOscRegion.astPoint[dwPI].s16X;
apolygon[dwPI].y = stOscRegion.astPoint[dwPI].s16Y;
}
point.x = pstPos->stPt.s16X;
point.y = pstPos->stPt.s16Y;
if (!InsidePolygon(apolygon, s32PntNum, point))
{
s32Ret = 1;
}
return s32Ret;
}
bool IntersectedPolygon(Vector3 vPoly[], Vector3 vLine[], int verticeCount)
{
Vector3 vNormal;
float originDistance = 0;
if(!IntersectedPlane(vPoly, vLine,vNormal,originDistance))
return false;
Vector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);//计算交点
if(InsidePolygon(vIntersection, vPoly, verticeCount))//找到交点，判断是否在多边形内
return true;
return false;
}
IMP_S32 ipOutsideOfRegion(POLYGON_REGION_S region, PEA_DETECTED_REGION_S *pstDrg, IMP_U8 *pu8Data, IMP_S32 s32W)
{
IMP_S32 lb_x, lb_y, ru_x, ru_y;
IMP_S32 xx, yy, dwPI;
IMP_S32 s32SpclFlag = 1;
IMP_S32 s32Sn0, s32PntNum;
Point apolygon[10], point;
s32PntNum = region.s32PointNum;
for (dwPI = 0; dwPI < s32PntNum; dwPI++)
{
apolygon[dwPI].x = region.astPoint[dwPI].s16X;
apolygon[dwPI].y = region.astPoint[dwPI].s16Y;
}
// pstDrg = pstTarget->stTargetImage.pstDRegions->astDrg + pstTarget->stTargetImage.s32DrgIndex;
//check if the point inside the Polygon
lb_x = pstDrg->stRect.s16X1;
ru_x = pstDrg->stRect.s16X2;
lb_y = pstDrg->stRect.s16Y1;
ru_y = pstDrg->stRect.s16Y2;
s32Sn0 = pstDrg->u8Sign;
// pu8Data = pstImgFg->pu8Data + lb_y*pstImgFg->s32W;
pu8Data += lb_y * s32W;
s32SpclFlag = 1;
for( yy=lb_y; yy<=ru_y; yy++ )
{
for( xx=lb_x; xx<=ru_x; xx++ )
{
if( pu8Data[xx]==s32Sn0)
{
point.x = xx;
point.y = yy;
if (!InsidePolygon(apolygon, s32PntNum, point))
{
s32SpclFlag = 0;
return s32SpclFlag;
}
}
}
pu8Data += s32W;
}
return s32SpclFlag;
}
bool SpherePolygonCollision(CVector3 vPolygon[],
CVector3 &vCenter, int vertexCount, float radius)
{
// 1) STEP ONE - Finding the sphere's classification
// Let's use our Normal() function to return us the normal to this polygon
CVector3 vNormal = Normal(vPolygon);
// This will store the distance our sphere is from the plane
float distance = 0.0f;
// This is where we determine if the sphere is in FRONT, BEHIND, or INTERSECTS the plane
int classification = ClassifySphere(vCenter, vNormal, vPolygon[0], radius, distance);
// If the sphere intersects the polygon's plane, then we need to check further
if(classification == INTERSECTS)
{
// 2) STEP TWO - Finding the psuedo intersection point on the plane
// Now we want to project the sphere's center onto the polygon's plane
CVector3 vOffset = vNormal * distance;
// Once we have the offset to the plane, we just subtract it from the center
// of the sphere. "vPosition" now a point that lies on the plane of the polygon.
CVector3 vPosition = vCenter - vOffset;
// 3) STEP THREE - Check if the intersection point is inside the polygons perimeter
// If the intersection point is inside the perimeter of the polygon, it returns true.
// We pass in the intersection point, the list of vertices and vertex count of the poly.
if(InsidePolygon(vPosition, vPolygon, 3))
return true; // We collided!
else
{
// 4) STEP FOUR - Check the sphere intersects any of the polygon's edges
// If we get here, we didn't find an intersection point in the perimeter.
// We now need to check collision against the edges of the polygon.
if(EdgeSphereCollision(vCenter, vPolygon, vertexCount, radius))
{
return true; // We collided!
}
}
}
// If we get here, there is obviously no collision
return false;
}
Bool_t TGeoArb8::Contains(Double_t *point) const
{
// test if point is inside this sphere
// first check Z range
if (TMath::Abs(point[2]) > fDz) return kFALSE;
// compute intersection between Z plane containing point and the arb8
Double_t poly[8];
#ifdef ALLUNROLLED
Double_t x1,y1,x2,y2,cross,x,y;
#endif
// memset(&poly[0], 0, 8*sizeof(Double_t));
//SetPlaneVertices(point[2], &poly[0]);
Double_t cf = 0.5*(fDz-point[2])/fDz;
Int_t i;
#ifndef ALLUNROLLED
for (i=0; i<4; i++) {
poly[2*i] = fXY[i+4][0]+cf*(fXY[i][0]-fXY[i+4][0]);
poly[2*i+1] = fXY[i+4][1]+cf*(fXY[i][1]-fXY[i+4][1]);
}
return InsidePolygon(point[0],point[1],poly);
#else
x=point[0]; y=point[1];
poly[0] = fXY[4][0] +cf*(fXY[0][0]-fXY[4][0]);
poly[1] = fXY[4][1] +cf*(fXY[0][1]-fXY[4][1]);
poly[2] = fXY[5][0] +cf*(fXY[1][0]-fXY[5][0]);
poly[3] = fXY[5][1] +cf*(fXY[1][1]-fXY[5][1]);
poly[4] = fXY[6][0] +cf*(fXY[2][0]-fXY[6][0]);
poly[5] = fXY[6][1] +cf*(fXY[2][1]-fXY[6][1]);
poly[6] = fXY[7][0] +cf*(fXY[3][0]-fXY[7][0]);
poly[7] = fXY[7][1] +cf*(fXY[3][1]-fXY[7][1]);
x1 = poly[0]; y1 = poly[1]; x2 = poly[2]; y2 = poly[3];
cross = (x-x1)*(y2-y1) - (y-y1)*(x2-x1);
if (cross<0) return kFALSE;
x1 = poly[2]; y1 = poly[3]; x2 = poly[4]; y2 = poly[5];
cross = (x-x1)*(y2-y1) - (y-y1)*(x2-x1);
if (cross<0) return kFALSE;
x1 = poly[4]; y1 = poly[5]; x2 = poly[6]; y2 = poly[7];
cross = (x-x1)*(y2-y1) - (y-y1)*(x2-x1);
if (cross<0) return kFALSE;
x1 = poly[6]; y1 = poly[7]; x2 = poly[0]; y2 = poly[1];
cross = (x-x1)*(y2-y1) - (y-y1)*(x2-x1);
if (cross<0) return kFALSE;
return kTRUE;
#endif
}
bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount)
{
CVector3 vNormal;
float originDistance = 0;
// First, make sure our line intersects the plane
// Reference // Reference
if(!IntersectedPlane(vPoly, vLine, vNormal, originDistance))
return false;
// Now that we have our normal and distance passed back from IntersectedPlane(),
// we can use it to calculate the intersection point.
CVector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);
// Now that we have the intersection point, we need to test if it's inside the polygon.
if(InsidePolygon(vIntersection, vPoly, verticeCount))
return true; // We collided! Return success
return false; // There was no collision, so return false
}
bool SpherePolygonCollision(Vector3 vPolygon[],
Vector3 &vCenter, int vertexCount, float radius)
{
Vector3 vNormal = Normal(vPolygon);
float distance = 0.0f; //球体到平面的距离
int classification = ClassifySphere(vCenter, vNormal, vPolygon[0], radius, distance);
if(classification == INTERSECTS)
{
Vector3 vOffset = vNormal * distance; //投影球体中心到平面
Vector3 vPosition = vCenter - vOffset;
if(InsidePolygon(vPosition, vPolygon, 3)) //判断交点是否在多变形内
return true;
else
{
if(EdgeSphereCollision(vCenter, vPolygon, vertexCount, radius)) //检查边
{
return true;
}
}
}
return false;
}
static IMP_S32 ipOscIsAccordantSpclRg( RULE_PARA_OSC_S *pstOscPara, IpOscPara *pstPara, IpTrackedTarget *pstTarget, IMP_S32 s32ZoneIndex)
{
Point apolygon[10], point;
IMP_S32 dwPI, s32PntNum;
IMP_S32 s32SizeFlag = 0;
IMP_S32 s32TimeFlag = 0;
IMP_S32 s32SpclFlag = 0;
IMP_S32 s32Valid = 0;
IMP_S32 s32Ret = 0;
IMP_S32 s32TargetArea;
IpTargetPosition *pstPos1, *pstPos2;
IMP_S32 s32Len, s32Life;
OSC_OBJ_LMT_SPECL_REGIONS_S *pstSpclRgs = pstOscPara->astSpclRgs;
IMP_S32 s32Index = 0;
IMP_S32 s32InOsc = 0;
IpOscTargetData *pstTargetData = ipGetOscTargetData( pstTarget->stPrivateData.pDataAnalyst );
IMP_S32 s32LeaveLimit = pstPara->s32LeaveLimit;
GRAY_IMAGE_S *pstImgFg = pstTarget->stTargetImage.pstDRegions->pstImgFgRgn;
PEA_DETECTED_REGION_S *pstDrg = NULL;
IMP_U8 *pu8Data;
IMP_S32 s32ImgW = pstTarget->stTargetImage.pstDRegions->pstImgFgRgn->s32W;
IMP_S32 s32ImgH = pstTarget->stTargetImage.pstDRegions->pstImgFgRgn->s32H;
s32Len = ipTargetTrajectoryGetLength( &pstTarget->stTrajectory );
if( pstTargetData->s32IsFirstPos == 0 )
{
pstTargetData->s32IsFirstPos = 1;
pstTargetData->pastFirstPos[s32ZoneIndex] = ipTargetTrajectoryGetPosition( &pstTarget->stTrajectory, -s32Len+1 );
pstTargetData->s32FirstPosTime = pstTargetData->pastFirstPos[s32ZoneIndex]->u32Time;
}
pstPos1 = pstTargetData->pastFirstPos[s32ZoneIndex];
// pstPos1 = ipTargetTrajectoryGetPosition( &pstTarget->stTrajectory, -s32Len );
pstPos2 = ipTargetTrajectoryGetPosition( &pstTarget->stTrajectory, 0 );
s32Life = pstPos2->u32Time - pstTargetData->s32FirstPosTime;
for (s32Index = 0;s32Index<IMP_MAX_OSC_NUM;s32Index++)
{
if (!pstSpclRgs[s32Index].s32Valid)
continue;
s32PntNum = pstSpclRgs[s32Index].stOscRg.s32PointNum;
for (dwPI = 0; dwPI < s32PntNum; dwPI++)
{
apolygon[dwPI].x = pstSpclRgs[s32Index].stOscRg.astPoint[dwPI].s16X;
apolygon[dwPI].y = pstSpclRgs[s32Index].stOscRg.astPoint[dwPI].s16Y;
}
point.x = pstPos2->stPt.s16X;
point.y = pstPos2->stPt.s16Y;
s32InOsc = !InsidePolygon(apolygon, s32PntNum, point);
if (s32InOsc)
break;
}
// printf("%d:%d\n", pstTarget->u32TargetId, s32InOsc);
if (!s32InOsc)
{
#if PRINT_DBG || 0
printf("ipOscIsAccordantSpclRg:%d\n", s32Ret);
#endif
return s32Ret;
}
s32SpclFlag = 1;
for (s32Index = 0; s32Index < IMP_MAX_OSC_NUM; s32Index++)
{
if (!pstSpclRgs[s32Index].s32Valid)
continue;
if (pstSpclRgs[s32Index].stSpclRgA.s32Valid)
{
s32SpclFlag = ipOutsideOfRegion(pstSpclRgs[s32Index].stSpclRgA, pstTarget->stTargetImage.pstDRegions->astDrg + pstTarget->stTargetImage.s32DrgIndex, pstImgFg->pu8Data, pstImgFg->s32W);
// printf("---A:%d\n", s32SpclFlag);
if (!s32SpclFlag) break;
}
if (pstSpclRgs[s32Index].stSpclRgB.s32Valid)
{
s32SpclFlag = ipOutsideOfRegion(pstSpclRgs[s32Index].stSpclRgB, pstTarget->stTargetImage.pstDRegions->astDrg + pstTarget->stTargetImage.s32DrgIndex, pstImgFg->pu8Data, pstImgFg->s32W);
// printf("---B:%d\n", s32SpclFlag);
if (!s32SpclFlag) break;
}
if (pstSpclRgs[s32Index].stSpclRgC.s32Valid)
{
s32SpclFlag = ipOutsideOfRegion(pstSpclRgs[s32Index].stSpclRgC, pstTarget->stTargetImage.pstDRegions->astDrg + pstTarget->stTargetImage.s32DrgIndex, pstImgFg->pu8Data, pstImgFg->s32W);
// printf("---C:%d\n", s32SpclFlag);
if (!s32SpclFlag) break;
}
}
// printf("---%d, %d\n", s32Life, pstPara->s32SpclMinTime);
// if( s32Life >= pstPara->s32SpclMinTime )
// s32TimeFlag =1;
// s32TargetArea = ipGetTargetPixelArea(pstTarget);
// if ((IMP_FLOAT)s32TargetArea >= 0.75*(s32MaxX-s32MinX)*(s32MaxY-s32MinY) && s32TargetArea<= (s32MaxX-s32MinX)*(s32MaxY-s32MinY))
// {
// s32SizeFlag = 1;
// }
if (!s32SpclFlag)
{
return s32Ret;
}
else
{
// if (s32TimeFlag)// && s32SizeFlag)
// {
s32Ret = 1;
// }
// else
// {
// s32Ret = 0;
// }
}
#if PRINT_DBG || 0
printf("ipOscIsAccordantSpclRg:%d\n", s32Ret);
#endif
return s32Ret;
}
void CCamera::CheckCameraCollision(CVector3 *pVertices, int numOfVerts)
{
// This function is pretty much a direct rip off of SpherePolygonCollision()
// We needed to tweak it a bit though, to handle the collision detection once
// it was found, along with checking every triangle in the list if we collided.
// pVertices is the world data. If we have space partitioning, we would pass in
// the vertices that were closest to the camera. What happens in this function
// is that we go through every triangle in the list and check if the camera's
// sphere collided with it. If it did, we don't stop there. We can have
// multiple collisions so it's important to check them all. One a collision
// is found, we calculate the offset to move the sphere off of the collided plane.
// Go through all the triangles
for(int i = 0; i < numOfVerts; i += 3)
{
// Store of the current triangle we testing
CVector3 vTriangle[3] = { pVertices[i], pVertices[i+1], pVertices[i+2] };
// 1) STEP ONE - Finding the sphere's classification
// We want the normal to the current polygon being checked
CVector3 vNormal = Normal(vTriangle);
// This will store the distance our sphere is from the plane
float distance = 0.0f;
// This is where we determine if the sphere is in FRONT, BEHIND, or INTERSECTS the plane
int classification = ClassifySphere(m_vPosition, vNormal, vTriangle[0], m_radius, distance);
// If the sphere intersects the polygon's plane, then we need to check further
if(classification == INTERSECTS)
{
// 2) STEP TWO - Finding the psuedo intersection point on the plane
// Now we want to project the sphere's center onto the triangle's plane
CVector3 vOffset = vNormal * distance;
// Once we have the offset to the plane, we just subtract it from the center
// of the sphere. "vIntersection" is now a point that lies on the plane of the triangle.
CVector3 vIntersection = m_vPosition - vOffset;
// 3) STEP THREE - Check if the intersection point is inside the triangles perimeter
// We first check if our intersection point is inside the triangle, if not,
// the algorithm goes to step 4 where we check the sphere again the polygon's edges.
// We do one thing different in the parameters for EdgeSphereCollision though.
// Since we have a bulky sphere for our camera, it makes it so that we have to
// go an extra distance to pass around a corner. This is because the edges of
// the polygons are colliding with our peripheral view (the sides of the sphere).
// So it looks likes we should be able to go forward, but we are stuck and considered
// to be colliding. To fix this, we just pass in the radius / 2. Remember, this
// is only for the check of the polygon's edges. It just makes it look a bit more
// realistic when colliding around corners. Ideally, if we were using bounding box
// collision, cylinder or ellipses, this wouldn't really be a problem.
if(InsidePolygon(vIntersection, vTriangle, 3) ||
EdgeSphereCollision(m_vPosition, vTriangle, 3, m_radius / 2))
{
// If we get here, we have collided! To handle the collision detection
// all it takes is to find how far we need to push the sphere back.
// GetCollisionOffset() returns us that offset according to the normal,
// radius, and current distance the center of the sphere is from the plane.
vOffset = GetCollisionOffset(vNormal, m_radius, distance);
// Now that we have the offset, we want to ADD it to the position and
// view vector in our camera. This pushes us back off of the plane. We
// don't see this happening because we check collision before we render
// the scene.
m_vPosition = m_vPosition + vOffset;
m_vView = m_vView + vOffset;
}
}
}
}
