/**
* Orient head and eyes towards m_lookAt.
*/
void C_HL2MP_Player::UpdateLookAt( void )
{
// head yaw
if (m_headYawPoseParam < 0 || m_headPitchPoseParam < 0)
return;
// orient eyes
m_viewtarget = m_vLookAtTarget;
// blinking
if (m_blinkTimer.IsElapsed())
{
m_blinktoggle = !m_blinktoggle;
m_blinkTimer.Start( RandomFloat( 1.5f, 4.0f ) );
}
// Figure out where we want to look in world space.
QAngle desiredAngles;
Vector to = m_vLookAtTarget - EyePosition();
VectorAngles( to, desiredAngles );
// Figure out where our body is facing in world space.
QAngle bodyAngles( 0, 0, 0 );
bodyAngles[YAW] = GetLocalAngles()[YAW];
float flBodyYawDiff = bodyAngles[YAW] - m_flLastBodyYaw;
m_flLastBodyYaw = bodyAngles[YAW];
// Set the head's yaw.
float desired = AngleNormalize( desiredAngles[YAW] - bodyAngles[YAW] );
desired = clamp( desired, m_headYawMin, m_headYawMax );
m_flCurrentHeadYaw = ApproachAngle( desired, m_flCurrentHeadYaw, 130 * gpGlobals->frametime );
// Counterrotate the head from the body rotation so it doesn't rotate past its target.
m_flCurrentHeadYaw = AngleNormalize( m_flCurrentHeadYaw - flBodyYawDiff );
desired = clamp( desired, m_headYawMin, m_headYawMax );
SetPoseParameter( m_headYawPoseParam, m_flCurrentHeadYaw );
// Set the head's yaw.
desired = AngleNormalize( desiredAngles[PITCH] );
desired = clamp( desired, m_headPitchMin, m_headPitchMax );
m_flCurrentHeadPitch = ApproachAngle( desired, m_flCurrentHeadPitch, 130 * gpGlobals->frametime );
m_flCurrentHeadPitch = AngleNormalize( m_flCurrentHeadPitch );
SetPoseParameter( m_headPitchPoseParam, m_flCurrentHeadPitch );
}
//Main tracking Algorithm
void AnalysisModule::larvaFind(uchar * img, int imWidth, int imHeight, int frameInd){
input = cv::Mat(imHeight,imWidth,CV_8UC1,NULL);
input.data = img;
if(output.rows != imHeight | output.cols != imWidth) output.create(imHeight,imWidth,CV_8UC1);
int nextInd = (index+1)%sampleInd.size();
//for Profiling
tic();
sampleInd[nextInd] = frameInd;
sampleTime[nextInd] = frameInd * frameIntervalMS;
//On first image, automatically determine threshold level using the Otsu method
// Minimizes within group variance of thresholded classes. Should land on the best boundary between backlight and larva
if(index == -1) threshold = otsuThreshold(img,imWidth*imHeight);
//Can speed this up by applying to a roi bounding box a bit larger than the previous one
//Simple inverted binary threshold of the image
cv::threshold(input,output,threshold,255,CV_THRESH_BINARY_INV); profile[0] = toctic();
//Detect Contours in the binary image
cv::findContours(output,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE); profile[1] = toctic();
//No contours detected
if (contours.size() == 0) {
return;
}
//find contour with largest perimeter length
double maxLen = 0; int maxInd = -1;
double cLen;
for(int i=0; i<contours.size(); i++){
cLen = cv::arcLength(cv::Mat(contours[i]), false);
if(cLen >= maxLen){ maxLen = cLen; maxInd = i; };
}
//Check to make sure that the perimeter is a larva by simple size analysis
//(larva should have a certain perimeter length at 8.1um/pixel)
cLarva[nextInd] = contours[maxInd];
//calculate bounding box
bBox[nextInd] = cv::boundingRect(cv::Mat(cLarva[nextInd])); profile[2] = toctic();
//Calculate fourier coefficients
fourierDecompose(cLarva[nextInd],nFourier,fourier[nextInd]);
centroid[nextInd] = cv::Point2f(fourier[nextInd][0][AX],fourier[nextInd][0][AY]); profile[3] = toctic();
//Reconstruct the estimated boundary
fourierReconstruct(fourier[nextInd],cFit,fitRes); profile[4] = toctic();
//Calculate Curvature
perimeterCurvature(cFit,curve,fitRes/8); profile[5] = toctic();
//Find head and tail based on curvature minimums (small angle = sharp region)
findHeadTail(cFit,curve,headTail);
head[nextInd] = headTail[0];
tail[nextInd] = headTail[1]; profile[6] = toctic();
//Calculate Skeleton
skeletonCalc(cFit,skeleton,headTail,length[nextInd],neck[nextInd]); profile[7] = toctic();
//Calculate bearing and head angle to bearing
bodyAngles(tailBearingAngle[nextInd], headToBodyAngle[nextInd], head[nextInd], neck[nextInd], tail[nextInd]); profile[8] = toctic();
//Capture stage position
stagePos[nextInd] = cv::Point(gui->stageThread->xpos,gui->stageThread->ypos);
//Keep track of entire history with a sample every 30 frames
if((nextInd % 30) == 0){
fullTrack[(fullTrackInd+1)%fullTrack.size()].x = stagePos[nextInd].x/gui->stageThread->tickPerMM_X+centroid[nextInd].x*gui->camThread->umPerPixel/1000.0;
fullTrack[(fullTrackInd+1)%fullTrack.size()].y = stagePos[nextInd].y/gui->stageThread->tickPerMM_Y+centroid[nextInd].y*gui->camThread->umPerPixel/1000.0;
fullTrackStim[(fullTrackInd+1)%fullTrack.size()] = binStimMax;
binStimMax = 0; //updated from stimThread
fullTrackInd++;
}
//Calculate Velocities of head and tail
calcVelocities(nextInd);
//Spew out profiling info
//for(int i=0; i<9; i++) qDebug("%d: %.4fms",i,profile[i]*1000);
//qDebug("\n");
index++;
};
/**
* Orient head and eyes towards m_lookAt.
*/
void C_CHostage::UpdateLookAt( CStudioHdr *pStudioHdr )
{
if (!m_isInit)
{
m_isInit = true;
Initialize( );
}
// head yaw
if (m_headYawPoseParam < 0 || m_bodyYawPoseParam < 0 || m_headPitchPoseParam < 0)
return;
if (GetLeader())
{
m_lookAt = GetLeader()->EyePosition();
}
// orient eyes
m_viewtarget = m_lookAt;
// blinking
if (m_blinkTimer.IsElapsed())
{
m_blinktoggle = !m_blinktoggle;
m_blinkTimer.Start( RandomFloat( 1.5f, 4.0f ) );
}
// Figure out where we want to look in world space.
QAngle desiredAngles;
Vector to = m_lookAt - EyePosition();
VectorAngles( to, desiredAngles );
// Figure out where our body is facing in world space.
float poseParams[MAXSTUDIOPOSEPARAM];
GetPoseParameters( pStudioHdr, poseParams );
QAngle bodyAngles( 0, 0, 0 );
bodyAngles[YAW] = GetRenderAngles()[YAW] + RemapVal( poseParams[m_bodyYawPoseParam], 0, 1, m_bodyYawMin, m_bodyYawMax );
float flBodyYawDiff = bodyAngles[YAW] - m_flLastBodyYaw;
m_flLastBodyYaw = bodyAngles[YAW];
// Set the head's yaw.
float desired = AngleNormalize( desiredAngles[YAW] - bodyAngles[YAW] );
desired = clamp( desired, m_headYawMin, m_headYawMax );
m_flCurrentHeadYaw = ApproachAngle( desired, m_flCurrentHeadYaw, HOSTAGE_HEAD_TURN_RATE * gpGlobals->frametime );
// Counterrotate the head from the body rotation so it doesn't rotate past its target.
m_flCurrentHeadYaw = AngleNormalize( m_flCurrentHeadYaw - flBodyYawDiff );
desired = clamp( desired, m_headYawMin, m_headYawMax );
SetPoseParameter( pStudioHdr, m_headYawPoseParam, m_flCurrentHeadYaw );
// Set the head's yaw.
desired = AngleNormalize( desiredAngles[PITCH] );
desired = clamp( desired, m_headPitchMin, m_headPitchMax );
m_flCurrentHeadPitch = ApproachAngle( desired, m_flCurrentHeadPitch, HOSTAGE_HEAD_TURN_RATE * gpGlobals->frametime );
m_flCurrentHeadPitch = AngleNormalize( m_flCurrentHeadPitch );
SetPoseParameter( pStudioHdr, m_headPitchPoseParam, m_flCurrentHeadPitch );
SetPoseParameter( pStudioHdr, "head_roll", 0.0f );
}
