/***************************************************************************//**
* @brief Main function.
*
* @return Returns 0.
*******************************************************************************/
int main() {
MajorRev = 1;
MinorRev = 1;
RcRev = 1;
DriverEnable = TRUE;
LastEnable = FALSE;
/*Enable cache*/
Xil_ICacheEnable();
Xil_DCacheEnable();
/* Perform any required platform init */
/* including hardware reset to HDMI devices */
HAL_PlatformInit(XPAR_AXI_IIC_0_BASEADDR, XPAR_SCUTIMER_DEVICE_ID,
XPAR_SCUGIC_SINGLE_DEVICE_ID, XPAR_SCUTIMER_INTR);
/* Initialize ADI repeater software and h/w */
ADIAPI_TransmitterInit();
ADIAPI_TransmitterSetPowerMode(REP_POWER_UP);
StartCount = HAL_GetCurrentMsCount();
ADIAPI_TransmitterMain();
/*Initialize the HDMI Core with default display settings*/
SetVideoResolution(RESOLUTION_640x480);
/*
* Initialize CE engine
*/
xstatus = CE_init();
if (xstatus != XST_SUCCESS) {
xil_printf("Unable to initialize CE HA!\n");
return XST_FAILURE;
}
/*
* Initialize ME engine
*/
xstatus = ME_init();
if (xstatus != XST_SUCCESS) {
xil_printf("Unable to initialize ME HA!\n");
return XST_FAILURE;
}
/*
* Initialize EEE engine
*/
xstatus = EEE_init();
if (xstatus != XST_SUCCESS) {
xil_printf("Unable to initialize EEE HA!\n");
return XST_FAILURE;
}
/*
* Initialize ECE engine
*/
xstatus = ECE_init();
if (xstatus != XST_SUCCESS) {
xil_printf("Unable to initialize ECE HA!\n");
return XST_FAILURE;
}
/* Initialize the interrupt controller */
xstatus = ScuGicInterrupt_Init();
if (xstatus != XST_SUCCESS) {
xil_printf("Unable to initialize Interrupts\n");
return XST_FAILURE;
}
/*
* Initially configure CE
*/
XCe_SetWidth(&hlsCE, width);
XCe_SetHeight(&hlsCE, height);
XCe_SetCsd1(&hlsCE, csd1);
XCe_SetCsd2(&hlsCE, csd2);
XCe_SetEpsilon(&hlsCE, epsilon);
/*
* Initially configure ME
*/
XMatchingengine32_SetWidth_v(&hlsME, width);
XMatchingengine32_SetHeight_v(&hlsME, height);
XMatchingengine32_SetReadaddress1_v(&hlsME, CE_OUTPUT_1_BASEADDR);
XMatchingengine32_SetReadaddress2_v(&hlsME, CE_OUTPUT_2_BASEADDR);
XMatchingengine32_SetFeatureout_v(&hlsME, FEATURE_X1_BASEADDR);
/*
* Initially configure EEE
*/
XEee_SetFaddr_v(&hlsEEE, FEATURE_X1_BASEADDR);
XEee_SetVecoutaddr_v(&hlsEEE, PRE_COMP_VECTOR_HW);
XEee_SetModelcount_v(&hlsEEE, 300);
XEee_SetErrthres_v(&hlsEEE, 2);
XEee_SetResultaddr_v(&hlsEEE, RESULT_ADDR);
XEee_SetModeladdr_v(&hlsEEE, (u32) model_param_32);
/*
* No initial configuration for EEE
*/
XEee_SetFaddr_v(&hlsEEE, FEATURE_X1_BASEADDR);
XEee_SetVecoutaddr_v(&hlsEEE, PRE_COMP_VECTOR_HW);
XEee_SetModelcount_v(&hlsEEE, 300);
XEee_SetErrthres_v(&hlsEEE, 2);
XEee_SetResultaddr_v(&hlsEEE, RESULT_ADDR);
XEee_SetModeladdr_v(&hlsEEE, (u32) model_param_32);
/*
* Initial Camera/Still Image mode choose
*/
printf("-------------------------------------\n");
printf("----------------Menu-----------------\n");
printf("-------------------------------------\n");
printf("-----Camera mode: press 'v' or 'V'---\n");
printf("-Sill images mode: press 'i' or 'I'--\n");
printf("--Quit Processing: press 'q' or 'Q'--\n");
printf("-------------------------------------\n");
printf("-------------------------------------\n");
char ctmp = 0;
while (1) {
while (!XUartPs_IsReceiveData(UART_BASEADDR))
;
ctmp = inbyte();
if (ctmp == 'v' || ctmp == 'V') {
Camflag = 1;
printf("-------------------------------------\n");
printf("--------------Camera mode------------\n");
printf("-------------------------------------\n");
printf("-------------------------------------\n");
printf(
"You can switch to Still Image mode using 'i' or 'I' later\n");
printf("-------------------------------------\n");
break;
}
if (ctmp == 'i' || ctmp == 'I') {
Camflag = 0;
printf("-------------------------------------\n");
printf("------------Still image mode---------\n");
printf("-------------------------------------\n");
printf("-------------------------------------\n");
printf("You can switch to Camera mode using 'v' or 'V' later\n");
printf("-------------------------------------\n");
break;
}
if (ctmp == 'q' || ctmp == 'Q') {
printf("-----------Exiting Application-------\n");
return 0;
}
printf("No such Option!!\n");
}
/*
* Instruction book
*/
printf("-------------------------------------\n");
printf("-------------Instructions------------\n");
printf("-------------------------------------\n");
printf("With/W.o. processing: press 'o' or 'O'\n");
printf("---Pure software: press 's' or 'S'---\n");
printf("---Pure hardware: press 'h' or 'H'---\n");
printf("Toggle SW/HW for CE: press 'c' or 'C'\n");
printf("Toggle SW/HW for ME: press 'm' or 'M'\n");
printf("Toggle SW/HW for EEE: press 'e' or 'E'\n");
printf("Toggle SW/HW for ECE: press 'f' or 'F'\n");
printf("--------Show CE output: press '1'--------\n");
printf("--------Show ME output: press '2'--------\n");
printf("------Show final output: press '3'-------\n");
printf("--Quit Processing: press 'q' or 'Q'--\n");
printf("-------------------------------------\n");
printf("-------------------------------------\n");
printf("-----------------------------------------\n");
printf("Special instructions for Still Image mode\n");
printf("-----------------------------------------\n");
printf("Start from the first frame: press up arrow\n");
printf("---Go to the last frame: press down arrow\n");
printf("Processing the next frame: press right arrow\n");
printf("Processing the previous frame: press left arrow\n");
printf("-----------------------------------------\n");
printf("-----------------------------------------\n");
/*
* Main loop
*/
while (config()) {
if (Oflag) {
if (Camflag) {
while (FRAME_INTR == 0)
;
FRAME_INTR = 0;
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, OUTPUT_BASEADDR);
} else {
ConvGray8ToGray32(640, 480, GET_INPUT_ADDR(fnum+1),
OUTPUT_BASEADDR);
}
} else {
if (!ceflag) {
if (!Camflag) {
printf("Still image mode, Software CE processing\n");
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
ImageSmoothGray8(640, 480, GET_INPUT_ADDR(fnum),
CE_BYTE_1_BASEADDR);
CensusEngine8to32(640, 480, 20, 3, 7, CE_BYTE_1_BASEADDR,
CE_OUTPUT_1_BASEADDR);
count = get_cyclecount() - count;
printf("CE software processing 1 time:%f \n",
((float) count) / CPUFREQ * 1000);
ceswcount++;
ceswsum += count;
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
ImageSmoothGray8(640, 480, GET_INPUT_ADDR(fnum+1),
CE_BYTE_2_BASEADDR);
CensusEngine8to32(640, 480, 20, 3, 7, CE_BYTE_2_BASEADDR,
CE_OUTPUT_2_BASEADDR);
count = get_cyclecount() - count;
printf("CE software processing 2 time:%f \n",
((float) count) / CPUFREQ * 1000);
ceswcount++;
ceswsum += count;
Xil_DCacheFlush();
} else {
printf("Camera mode, Software CE processing\n");
while (FRAME_INTR == 0)
;
FRAME_INTR = 0;
Camera24ToGray8(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, CE_PROC_1_BASEADDR);
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
ImageSmoothGray8(640, 480, CE_PROC_1_BASEADDR,
CE_BYTE_1_BASEADDR);
CensusEngine8to32(640, 480, 20, 3, 7, CE_BYTE_1_BASEADDR,
CE_OUTPUT_1_BASEADDR);
count = get_cyclecount() - count;
printf("CE software processing 1 time:%f \n",
((float) count) / CPUFREQ * 1000);
ceswcount++;
ceswsum += count;
while (FRAME_INTR == 0)
;
FRAME_INTR = 0;
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, BUF_CAM);
Camera24ToGray8(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, CE_PROC_2_BASEADDR);
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
ImageSmoothGray8(640, 480, CE_PROC_2_BASEADDR,
CE_BYTE_2_BASEADDR);
CensusEngine8to32(640, 480, 20, 3, 7, CE_BYTE_2_BASEADDR,
CE_OUTPUT_2_BASEADDR);
count = get_cyclecount() - count;
printf("CE software processing 2 time:%f \n",
((float) count) / CPUFREQ * 1000);
ceswcount++;
ceswsum += count;
Xil_DCacheFlush();
}
} else {
if (!XCe_IsReady(&hlsCE)) {
DBG_MSG(
"!!! HLS_CE peripheral is not ready! Exiting...\n\r");
return XST_FAILURE;
}
if (!Camflag) {
printf("Still image mode, Hardware CE processing\n");
XCe_SetReadaddress(&hlsCE, GET_INPUT_ADDR(fnum));
} else {
printf("Camera mode, Hardware CE processing\n");
while (FRAME_INTR == 0)
;
FRAME_INTR = 0;
Camera24ToGray8(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, CE_PROC_1_BASEADDR);
Xil_DCacheFlush();
XCe_SetReadaddress(&hlsCE, CE_PROC_1_BASEADDR);
}
XCe_SetWriteaddress(&hlsCE, CE_OUTPUT_1_BASEADDR);
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
CE_start();
while (CEisdone == 0)
;
CEisdone = 0;
count = get_cyclecount() - count;
printf("CE hardware processing 1 time:%f \n",
((float) count) / CPUFREQ * 1000);
cehwcount++;
cehwsum += count;
if (!XCe_IsReady(&hlsCE)) {
DBG_MSG(
"!!! HLS_CE peripheral is not ready! Exiting...\n\r");
return XST_FAILURE;
}
if (!Camflag) {
XCe_SetReadaddress(&hlsCE, GET_INPUT_ADDR(fnum+1));
} else {
while (FRAME_INTR == 0)
;
FRAME_INTR = 0;
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, BUF_CAM);
Camera24ToGray8(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
VIDEO_BASEADDR, CE_PROC_2_BASEADDR);
Xil_DCacheFlush();
XCe_SetReadaddress(&hlsCE, CE_PROC_2_BASEADDR);
}
XCe_SetWriteaddress(&hlsCE, CE_OUTPUT_2_BASEADDR);
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
CE_start();
while (CEisdone == 0)
;
CEisdone = 0;
count = get_cyclecount() - count;
printf("CE hardware processing 2 time:%f \n",
((float) count) / CPUFREQ * 1000);
cehwcount++;
cehwsum += count;
}
if (!meflag) {
if (!Camflag) {
printf("Still image mode, Software ME processing\n");
} else {
printf("Camera mode, Software ME processing\n");
}
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
matchcount = MatchingEngine32HWO(640, 480, 7, 7,
CE_OUTPUT_1_BASEADDR, CE_OUTPUT_2_BASEADDR,
FEATURE_X1_BASEADDR);
count = get_cyclecount() - count;
printf("ME software processing time:%f \n",
((float) count) / CPUFREQ * 1000);
printf("matchcount: %d\n", matchcount);
meswcount++;
meswsum += count;
Xil_DCacheFlush();
} else {
if (!Camflag) {
printf("Still image mode, Hardware ME processing\n");
} else {
printf("Camera mode, Hardware ME processing\n");
}
if (!XMatchingengine32_IsReady(&hlsME)) {
DBG_MSG(
"!!! HLS_ME peripheral is not ready! Exiting...\n\r");
return XST_FAILURE;
}
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
ME_start();
while (MEisdone == 0)
;
MEisdone = 0;
count = get_cyclecount() - count;
printf("ME hardware processing time:%f \n",
((float) count) / CPUFREQ * 1000);
matchcount = XMatchingengine32_GetReturn(&hlsME);
printf("matchcount: %d\n", matchcount);
mehwcount++;
mehwsum += count;
}
if (!Camflag) {
ConvGray8ToGray32(640, 480, GET_INPUT_ADDR(fnum+1),
OUTPUT_ME_BASEADDR);
DrawVector32hw(640, 480, FEATURE_X1_BASEADDR, matchcount,
OUTPUT_ME_BASEADDR, 0);
Xil_DCacheFlush();
}
presample(FEATURE_X1_BASEADDR, FEATURE_IMG, matchcount, 640, 480);
if (!eeeflag) {
if (!Camflag) {
printf("Still image mode, Software EEE processing\n");
} else {
printf("Camera mode, Software EEE processing\n");
}
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
samplecount = sample_flow_vectors(FEATURE_IMG, COMP_VECTOR, 640,
480, 2, 8);
ret = estimate_ego_motion_first_order_flow(COMP_VECTOR,
samplecount, model_param_est, 300, 300, 2, 0.75F);
count = get_cyclecount() - count;
printf("SW EEE consuming time: %f ms\n",
((float) count) / CPUFREQ * 1000);
printf("samplecount: %d\n", samplecount);
if (ret) {
printf("Software EEE succeeds!!\n");
eeeswcount++;
eeeswsum += count;
Xil_DCacheFlush();
} else {
printf("Software EEE fails!!\n");
}
} else {
if (!Camflag) {
printf("Still image mode, Hardware EEE processing\n");
} else {
printf("Camera mode, Hardware EEE processing\n");
}
if (!XEee_IsReady(&hlsEEE)) {
DBG_MSG(
"!!! HLS_EEE peripheral is not ready! Exiting...\n\r");
return XST_FAILURE;
}
XEee_SetMatchcount_v(&hlsEEE, matchcount);
XEee_SetMode_v(&hlsEEE, 0);
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
EEE_start();
while (EEEisdone == 0)
;
EEEisdone = 0;
samplecount = compressvectorHW(PRE_COMP_VECTOR_HW,
COMP_VECTOR_HW, 80, 60);
ret = estimate_ego_motion_first_order_flow_HW( COMP_VECTOR_HW,
samplecount, model_param_est, model_param_32, 300, 300);
Xil_DCacheFlush();
if (ret) {
if (!XEee_IsReady(&hlsEEE)) {
DBG_MSG(
"!!! HLS_EEE peripheral is not ready! Exiting...\n\r");
return XST_FAILURE;
}
XEee_SetMode_v(&hlsEEE, 1);
XEee_SetModeladdr_v(&hlsEEE, (u32) model_param_32);
EEE_start();
while (EEEisdone == 0)
;
EEEisdone = 0;
count = get_cyclecount() - count;
printf("HW EEE consuming time: %f ms\n",
((float) count) / CPUFREQ * 1000);
printf("samplecount: %d\n", samplecount);
eeehwcount++;
eeehwsum += count;
int validvectors = Xil_In32(RESULT_ADDR) >> 16;
int betteroutlier = Xil_In32(RESULT_ADDR) & 0xffff;
bmid = Xil_In32(RESULT_ADDR + 4);
printf(" XC: %f \n\r",
((float) model_param_32[4 * bmid]) / 8.0);
printf(" YC: %f \n\r",
((float) model_param_32[4 * bmid + 1]) / 8.0);
printf(" D : %f \n\r",
((float) model_param_32[4 * bmid + 2])
/ 2147483648.0);
printf(" R : %f \n\r",
((float) model_param_32[4 * bmid + 3])
/ 2147483648.0);
printf(" Total iterations : %d \n\r", 300);
printf(" Outlier count : %d \n\r", betteroutlier);
printf(" Total count : %d \n\r", validvectors);
printf(" Outlier Ratio : %f \n\r",
((float) betteroutlier) / ((float) validvectors));
printf("HW EEE succeeds!!\n");
} else {
count = get_cyclecount() - count;
printf("HW EEE consuming time: %f ms\n",
((float) count) / CPUFREQ * 1000);
printf("samplecount: %d\n", samplecount);
printf("HW EEE fails!!\n");
}
}
if (ret) {
if (!eceflag) {
if (eeeflag) {
model_param_est[0] = ((float) model_param_32[4 * bmid])
/ 8.0;
model_param_est[1] = ((float) model_param_32[4 * bmid
+ 1]) / 8.0;
model_param_est[2] = ((float) model_param_32[4 * bmid
+ 2]) / 2147483648.0;
model_param_est[3] = ((float) model_param_32[4 * bmid
+ 3]) / 2147483648.0;
}
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
samplecount = sample_flow_vectors(FEATURE_IMG, FULL_VECTOR,
640, 480, 0, 1);
gen_firstOrderFlow_vectors_4(model_param_est, samplecount,
FULL_VECTOR, RES_VECTOR);
diff_motion_vectors(FULL_VECTOR, RES_VECTOR, samplecount,
RES_VECTOR, 2);
count = get_cyclecount() - count;
printf("SW ECE consuming time: %f ms\n",
((float) count) / CPUFREQ * 1000);
if (!Camflag) {
ConvGray8ToGray32(640, 480, GET_INPUT_ADDR(fnum+1),
OUTPUT_BASEADDR);
} else {
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
BUF_CAM, OUTPUT_BASEADDR);
}
DrawVector32eee(640, 480, samplecount, RES_VECTOR,
OUTPUT_BASEADDR, 0);
Xil_DCacheFlush();
eceswcount++;
eceswsum += count;
} else {
if (!XEce_IsReady(&hlsECE)) {
DBG_MSG(
"!!! HLS_ECE peripheral is not ready! Exiting...\n\r");
return XST_FAILURE;
}
XEce_SetFullvectoraddr_v(&hlsECE, FEATURE_X1_BASEADDR);
XEce_SetVeccount_v(&hlsECE, matchcount);
XEce_SetResultvectoraddr_v(&hlsECE, FEATURE_X2_BASEADDR);
XEce_SetErrthres_v(&hlsECE, 2);
if (!eeeflag) {
model_param_32_tmp[0] = (int) (model_param_est[0] * 8);
model_param_32_tmp[1] = (int) (model_param_est[1] * 8);
model_param_32_tmp[2] = (int) (model_param_est[2]
* 2147483648);
model_param_32_tmp[3] = (int) (model_param_est[3]
* 2147483648);
} else {
model_param_32_tmp[0] = model_param_32[4 * bmid];
model_param_32_tmp[1] = model_param_32[4 * bmid + 1];
model_param_32_tmp[2] = model_param_32[4 * bmid + 2];
model_param_32_tmp[3] = model_param_32[4 * bmid + 3];
}
Xil_Out32(BEST_MODEL, model_param_32_tmp[0]);
Xil_Out32(BEST_MODEL + 4, model_param_32_tmp[1]);
Xil_Out32(BEST_MODEL + 8, model_param_32_tmp[2]);
Xil_Out32(BEST_MODEL + 12, model_param_32_tmp[3]);
Xil_DCacheFlush();
XEce_SetBestmodeladdr_v(&hlsECE, BEST_MODEL);
init_perfcounters(1, 0);
EnablePerfCounters();
count = get_cyclecount();
ECE_start();
while (ECEisdone == 0)
;
ECEisdone = 0;
count = get_cyclecount() - count;
matchcount2 = XEce_GetReturn(&hlsECE);
printf("HW ECE consuming time: %f ms\n",
((float) count) / CPUFREQ * 1000);
printf("Rest vector numbers: %d \n", matchcount2);
if (!Camflag) {
ConvGray8ToGray32(640, 480, GET_INPUT_ADDR(fnum+1),
OUTPUT_BASEADDR);
} else {
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
BUF_CAM, OUTPUT_BASEADDR);
}
DrawVector32hw(640, 480, FEATURE_X2_BASEADDR, matchcount2,
OUTPUT_BASEADDR, 0);
Xil_DCacheFlush();
ecehwcount++;
ecehwsum += count;
}
} else {
printf("Because of EEE failure, no compensation done!!\n");
if (!Camflag) {
ConvGray8ToGray32(640, 480, GET_INPUT_ADDR(fnum+1),
OUTPUT_BASEADDR);
} else {
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
BUF_CAM, OUTPUT_BASEADDR);
}
DrawVector32hw(640, 480, FEATURE_X1_BASEADDR, matchcount,
OUTPUT_BASEADDR, 0);
Xil_DCacheFlush();
}
}
if (!Camflag) {
ConfigHdmiVDMA(detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
OUTPUT_BASEADDR);
if (ATV_GetElapsedMs(StartCount, NULL) >= HDMI_CALL_INTERVAL_MS) {
StartCount = HAL_GetCurrentMsCount();
if (APP_DriverEnabled()) {
ADIAPI_TransmitterMain();
}
}
} else {
if (camceflag) {
ConfigHdmiVDMA(detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
CE_OUTPUT_1_BASEADDR);
} else if (cammeflag) {
CameraCopy(640, 480,
detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
BUF_CAM, OUTPUT_ME_BASEADDR);
DrawVector32hw(640, 480, FEATURE_X1_BASEADDR, matchcount,
OUTPUT_ME_BASEADDR, 0);
Xil_DCacheFlush();
ConfigHdmiVDMA(detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
OUTPUT_ME_BASEADDR);
} else {
ConfigHdmiVDMA(detailedTiming[currentResolution][H_ACTIVE_TIME],
detailedTiming[currentResolution][V_ACTIVE_TIME],
OUTPUT_BASEADDR);
}
if (ATV_GetElapsedMs(StartCount, NULL) >= HDMI_CALL_INTERVAL_MS) {
StartCount = HAL_GetCurrentMsCount();
if (APP_DriverEnabled()) {
ADIAPI_TransmitterMain();
}
}
}
}
// COMPUTE ======================================
MStatus gear_rollSplineKine::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != output)
return MS::kUnknownParameter;
// Get inputs matrices ------------------------------
// Inputs Parent
MArrayDataHandle adh = data.inputArrayValue( ctlParent );
int count = adh.elementCount();
if (count < 1)
return MS::kFailure;
MMatrixArray inputsP(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputsP[i] = adh.inputValue().asMatrix();
}
// Inputs
adh = data.inputArrayValue( inputs );
if (count != adh.elementCount())
return MS::kFailure;
MMatrixArray inputs(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputs[i] = adh.inputValue().asMatrix();
}
adh = data.inputArrayValue( inputsRoll );
if (count != adh.elementCount())
return MS::kFailure;
MDoubleArray roll(adh.elementCount());
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
roll[i] = degrees2radians((double)adh.inputValue().asFloat());
}
// Output Parent
MDataHandle ha = data.inputValue( outputParent );
MMatrix outputParent = ha.asMatrix();
// Get inputs sliders -------------------------------
double in_u = (double)data.inputValue( u ).asFloat();
bool in_resample = data.inputValue( resample ).asBool();
int in_subdiv = data.inputValue( subdiv ).asShort();
bool in_absolute = data.inputValue( absolute ).asBool();
// Process ------------------------------------------
// Get roll, pos, tan, rot, scl
MVectorArray pos(count);
MVectorArray tan(count);
MQuaternion *rot;
rot = new MQuaternion[count];
MVectorArray scl(count);
double threeDoubles[3];
for (int i = 0 ; i < count ; i++){
MTransformationMatrix tp(inputsP[i]);
MTransformationMatrix t(inputs[i]);
pos[i] = t.getTranslation(MSpace::kWorld);
rot[i] = tp.rotation();
t.getScale(threeDoubles, MSpace::kWorld);
scl[i] = MVector(threeDoubles[0], threeDoubles[1], threeDoubles[2]);
tan[i] = MVector(threeDoubles[0] * 2.5, 0, 0).rotateBy(t.rotation());
}
// Get step and indexes
// We define between wich controlers the object is to be able to
// calculate the bezier 4 points front this 2 objects
double step = 1.0 / max( 1, count-1.0 );
int index1 = (int)min( count-2.0, in_u/step );
int index2 = index1+1;
int index1temp = index1;
int index2temp = index2;
double v = (in_u - step * double(index1)) / step;
double vtemp = v;
// calculate the bezier
MVector bezierPos;
MVector xAxis, yAxis, zAxis;
if(!in_resample){
// straight bezier solve
MVectorArray results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
bezierPos = results[0];
xAxis = results[1];
}
else if(!in_absolute){
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[index1];
presampletan[0] = tan[index1];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results(2);
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],sampleu);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(v>=samplelen[i] && v <= samplelen[i+1]){
v = (v - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],v);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],v);
break;
}
}
}
else{
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[0];
presampletan[0] = tan[0];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results;
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
index1 = (int)min(count-2,sampleu / step);
index2 = index1+1;
v = (sampleu - step * double(index1)) / step;
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(in_u>=samplelen[i] && in_u <= samplelen[i+1]){
in_u = (in_u - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],in_u);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],in_u);
break;
}
}
}
// compute the scaling (straight interpolation!)
MVector scl1 = linearInterpolate(scl[index1temp], scl[index2temp],vtemp);
// compute the rotation!
MQuaternion q = slerp(rot[index1temp], rot[index2temp], vtemp);
yAxis = MVector(0,1,0);
yAxis = yAxis.rotateBy(q);
// use directly or project the roll values!
// print roll
double a = linearInterpolate(roll[index1temp], roll[index2temp], vtemp);
yAxis = yAxis.rotateBy( MQuaternion(xAxis.x * sin(a/2.0), xAxis.y * sin(a/2.0), xAxis.z * sin(a/2.0), cos(a/2.0)));
zAxis = xAxis ^ yAxis;
zAxis.normalize();
yAxis = zAxis ^ xAxis;
yAxis.normalize();
// Output -------------------------------------------
MTransformationMatrix result;
// translation
result.setTranslation(bezierPos, MSpace::kWorld);
// rotation
q = getQuaternionFromAxes(xAxis,yAxis,zAxis);
result.setRotationQuaternion(q.x, q.y, q.z, q.w);
// scaling
threeDoubles[0] = 1;
threeDoubles[0] = scl1.y;
threeDoubles[0] = scl1.z;
result.setScale(threeDoubles, MSpace::kWorld);
MDataHandle h = data.outputValue( output );
h.setMMatrix( result.asMatrix() * outputParent.inverse() );
data.setClean( plug );
return MS::kSuccess;
}
