From edcc3e250c7f7f178543a932af2e63d61b40aa4a Mon Sep 17 00:00:00 2001 From: Tang1705 <17301138@bjtu.edu.cn> Date: Tue, 5 May 2020 23:30:52 +0800 Subject: [PATCH] add core files --- Classes/API.cpp | 1833 ++++++++++++++++++++++++++++++++++ Classes/API.h | 259 +++++ Classes/CalibrationData.cpp | 179 ++++ Classes/CalibrationData.h | 49 + Classes/Calibrator.cpp | 338 +++++++ Classes/Calibrator.h | 50 + Classes/Camera.cpp | 28 + Classes/Camera.h | 60 ++ Classes/CameraArguments.cpp | 4 +- Classes/CameraPointGrey.cpp | 306 ++++++ Classes/CameraPointGrey.h | 28 + Classes/Common.h | 108 ++ Classes/CoreAlgorithm.cpp | 591 +++++++---- Classes/CoreAlgorithm.h | 61 +- Classes/Device.cpp | 47 + Classes/Device.h | 28 + Classes/Help.cpp | 8 +- Classes/Loading.cpp | 4 +- Classes/Loading.h | 2 + Classes/MyThread.cpp | 2 + Classes/MyThread.h | 1 + Classes/Projector.h | 25 + Classes/ProjectorLC4500.cpp | 116 +++ Classes/ProjectorLC4500.h | 30 + Classes/Reconstruction.cpp | 430 ++++++-- Classes/Reconstruction.h | 27 +- Classes/TreeModel.cpp | 205 ++++ Classes/TreeModel.h | 68 ++ Classes/bsxfun.cpp | 94 ++ Classes/bsxfun.h | 27 + Classes/cvtools.cpp | 66 ++ Classes/cvtools.h | 18 + Classes/cwt.cpp | 291 ++++++ Classes/cwt.h | 27 + Classes/cwt_data.cpp | 33 + Classes/cwt_data.h | 28 + Classes/cwt_emxAPI.cpp | 130 +++ Classes/cwt_emxAPI.h | 32 + Classes/cwt_emxutil.cpp | 301 ++++++ Classes/cwt_emxutil.h | 36 + Classes/cwt_initialize.cpp | 32 + Classes/cwt_initialize.h | 26 + Classes/cwt_rtwutil.cpp | 73 ++ Classes/cwt_rtwutil.h | 26 + Classes/cwt_terminate.cpp | 31 + Classes/cwt_terminate.h | 26 + Classes/cwt_types.h | 67 ++ Classes/cwtfilterbank.cpp | 997 ++++++++++++++++++ Classes/cwtfilterbank.h | 29 + Classes/cwtfreqlimits.cpp | 149 +++ Classes/cwtfreqlimits.h | 27 + Classes/fft.cpp | 69 ++ Classes/fft.h | 27 + Classes/fft1.cpp | 1073 ++++++++++++++++++++ Classes/fft1.h | 41 + Classes/fstools.cpp | 31 + Classes/fstools.h | 13 + Classes/gammaln.cpp | 163 +++ Classes/gammaln.h | 26 + Classes/hid.Win.c | 936 +++++++++++++++++ Classes/hidapi.h | 385 +++++++ Classes/ifft.cpp | 191 ++++ Classes/ifft.h | 26 + Classes/log2.cpp | 51 + Classes/log2.h | 26 + Classes/main.cpp | 3 + Classes/rtGetInf.cpp | 54 + Classes/rtGetInf.h | 33 + Classes/rtGetNaN.cpp | 38 + Classes/rtGetNaN.h | 32 + Classes/rt_defines.h | 25 + Classes/rt_nonfinite.cpp | 79 ++ Classes/rt_nonfinite.h | 39 + Classes/rtwtypes.h | 43 + Classes/structured_light.cpp | 458 +++++++++ Classes/structured_light.h | 38 + Classes/tmwtypes.h | 757 ++++++++++++++ Classes/usb.cpp | 85 ++ Classes/usb.h | 28 + Classes/wavCFandSD.cpp | 142 +++ Classes/wavCFandSD.h | 27 + UI/Reconstruction.ui | 55 +- 82 files changed, 12139 insertions(+), 308 deletions(-) create mode 100644 Classes/API.cpp create mode 100644 Classes/API.h create mode 100644 Classes/CalibrationData.cpp create mode 100644 Classes/CalibrationData.h create mode 100644 Classes/Calibrator.cpp create mode 100644 Classes/Calibrator.h create mode 100644 Classes/Camera.cpp create mode 100644 Classes/Camera.h create mode 100644 Classes/CameraPointGrey.cpp create mode 100644 Classes/CameraPointGrey.h create mode 100644 Classes/Common.h create mode 100644 Classes/Device.cpp create mode 100644 Classes/Device.h create mode 100644 Classes/Projector.h create mode 100644 Classes/ProjectorLC4500.cpp create mode 100644 Classes/ProjectorLC4500.h create mode 100644 Classes/TreeModel.cpp create mode 100644 Classes/TreeModel.h create mode 100644 Classes/bsxfun.cpp create mode 100644 Classes/bsxfun.h create mode 100644 Classes/cvtools.cpp create mode 100644 Classes/cvtools.h create mode 100644 Classes/cwt.cpp create mode 100644 Classes/cwt.h create mode 100644 Classes/cwt_data.cpp create mode 100644 Classes/cwt_data.h create mode 100644 Classes/cwt_emxAPI.cpp create mode 100644 Classes/cwt_emxAPI.h create mode 100644 Classes/cwt_emxutil.cpp create mode 100644 Classes/cwt_emxutil.h create mode 100644 Classes/cwt_initialize.cpp create mode 100644 Classes/cwt_initialize.h create mode 100644 Classes/cwt_rtwutil.cpp create mode 100644 Classes/cwt_rtwutil.h create mode 100644 Classes/cwt_terminate.cpp create mode 100644 Classes/cwt_terminate.h create mode 100644 Classes/cwt_types.h create mode 100644 Classes/cwtfilterbank.cpp create mode 100644 Classes/cwtfilterbank.h create mode 100644 Classes/cwtfreqlimits.cpp create mode 100644 Classes/cwtfreqlimits.h create mode 100644 Classes/fft.cpp create mode 100644 Classes/fft.h create mode 100644 Classes/fft1.cpp create mode 100644 Classes/fft1.h create mode 100644 Classes/fstools.cpp create mode 100644 Classes/fstools.h create mode 100644 Classes/gammaln.cpp create mode 100644 Classes/gammaln.h create mode 100644 Classes/hid.Win.c create mode 100644 Classes/hidapi.h create mode 100644 Classes/ifft.cpp create mode 100644 Classes/ifft.h create mode 100644 Classes/log2.cpp create mode 100644 Classes/log2.h create mode 100644 Classes/rtGetInf.cpp create mode 100644 Classes/rtGetInf.h create mode 100644 Classes/rtGetNaN.cpp create mode 100644 Classes/rtGetNaN.h create mode 100644 Classes/rt_defines.h create mode 100644 Classes/rt_nonfinite.cpp create mode 100644 Classes/rt_nonfinite.h create mode 100644 Classes/rtwtypes.h create mode 100644 Classes/structured_light.cpp create mode 100644 Classes/structured_light.h create mode 100644 Classes/tmwtypes.h create mode 100644 Classes/usb.cpp create mode 100644 Classes/usb.h create mode 100644 Classes/wavCFandSD.cpp create mode 100644 Classes/wavCFandSD.h diff --git a/Classes/API.cpp b/Classes/API.cpp new file mode 100644 index 0000000..e70d4e8 --- /dev/null +++ b/Classes/API.cpp @@ -0,0 +1,1833 @@ +/* + * API.cpp + * + * This module provides C callable APIs for each of the command supported by LightCrafter4500 platform and detailed in the programmer's guide. + * + * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/ + * ALL RIGHTS RESERVED + * +*/ + +#include "API.h" +#include "string.h" +#include "usb.h" +#include "Common.h" +#include + +extern unsigned char OutputBuffer[]; +extern unsigned char InputBuffer[]; + +CmdFormat CmdList[255] = +{ + { 0x1A, 0x00, 0x01 }, //SOURCE_SEL, + { 0x1A, 0x02, 0x01 }, //PIXEL_FORMAT, + { 0x1A, 0x03, 0x01 }, //CLK_SEL, + { 0x1A, 0x37, 0x01 }, //CHANNEL_SWAP, + { 0x1A, 0x04, 0x01 }, //FPD_MODE, + { 0, 0, 0 }, //CURTAIN_COLOR, + { 0x02, 0x00, 0x01 }, //POWER_CONTROL, + { 0x10, 0x08, 0x01 }, //FLIP_LONG, + { 0x10, 0x09, 0x01 }, //FLIP_SHORT, + { 0x12, 0x03, 0x01 }, //TPG_SEL, + { 0x1A, 0x05, 0x01 }, //PWM_INVERT, + { 0x1A, 0x07, 0x01 }, //LED_ENABLE, + { 0x02, 0x05, 0x00 }, //GET_VERSION, + { 0x08, 0x02, 0x00 }, //SW_RESET, + { 0, 0, 0 }, //DMD_PARK, + { 0, 0, 0 }, //BUFFER_FREEZE, + { 0x1A, 0x0A, 0x00 }, //STATUS_HW, + { 0x1A, 0x0B, 0x00 }, //STATUS_SYS, + { 0x1A, 0x0C, 0x00 }, //STATUS_MAIN, + { 0, 0, 0 }, //CSC_DATA, + { 0, 0, 0 }, //GAMMA_CTL, + { 0, 0, 0 }, //BC_CTL, + { 0x1A, 0x10, 0x01 }, //PWM_ENABLE, + { 0x1A, 0x11, 0x06 }, //PWM_SETUP, + { 0x1A, 0x12, 0x05 }, //PWM_CAPTURE_CONFIG, + { 0x1A, 0x38, 0x02 }, //GPIO_CONFIG, + { 0x0B, 0x01, 0x03 }, //LED_CURRENT, + { 0x10, 0x00, 0x10 }, //DISP_CONFIG, + { 0, 0, 0 }, //TEMP_CONFIG, + { 0, 0, 0 }, //TEMP_READ, + { 0x1A, 0x16, 0x09 }, //MEM_CONTROL, + { 0, 0, 0 }, //I2C_CONTROL, + { 0x1A, 0x1A, 0x01 }, //LUT_VALID, + { 0x1A, 0x1B, 0x01 }, //DISP_MODE, + { 0x1A, 0x1D, 0x03 }, //TRIG_OUT1_CTL, + { 0x1A, 0x1E, 0x03 }, //TRIG_OUT2_CTL, + { 0x1A, 0x1F, 0x02 }, //RED_STROBE_DLY, + { 0x1A, 0x20, 0x02 }, //GRN_STROBE_DLY, + { 0x1A, 0x21, 0x02 }, //BLU_STROBE_DLY, + { 0x1A, 0x22, 0x01 }, //PAT_DISP_MODE, + { 0x1A, 0x23, 0x01 }, //PAT_TRIG_MODE, + { 0x1A, 0x24, 0x01 }, //PAT_START_STOP, + { 0, 0, 0 }, //BUFFER_SWAP, + { 0, 0, 0 }, //BUFFER_WR_DISABLE, + { 0, 0, 0 }, //CURRENT_RD_BUFFER, + { 0x1A, 0x29, 0x08 }, //PAT_EXPO_PRD, + { 0x1A, 0x30, 0x01 }, //INVERT_DATA, + { 0x1A, 0x31, 0x04 }, //PAT_CONFIG, + { 0x1A, 0x32, 0x01 }, //MBOX_ADDRESS, + { 0x1A, 0x33, 0x01 }, //MBOX_CONTROL, + { 0x1A, 0x34, 0x00 }, //MBOX_DATA, + { 0x1A, 0x35, 0x04 }, //TRIG_IN1_DELAY, + { 0, 0, 0 }, //TRIG_IN2_CONTROL, + { 0x1A, 0x39, 0x01 }, //SPLASH_LOAD, + { 0x1A, 0x3A, 0x02 }, //SPLASH_LOAD_TIMING, + { 0x08, 0x07, 0x03 }, //GPCLK_CONFIG, + { 0, 0, 0 }, //PULSE_GPIO_23, + { 0, 0, 0 }, //ENABLE_LCR_DEBUG, + { 0x12, 0x04, 0x0C }, //TPG_COLOR, + { 0x1A, 0x13, 0x05 }, //PWM_CAPTURE_READ, + { 0x30, 0x01, 0x00 }, //PROG_MODE, + { 0x00, 0x00, 0x00 }, //BL_STATUS + { 0x00, 0x23, 0x01 }, //BL_SPL_MODE + { 0x00, 0x15, 0x01 }, //BL_GET_MANID, + { 0x00, 0x15, 0x01 }, //BL_GET_DEVID, + { 0x00, 0x15, 0x01 }, //BL_GET_CHKSUM, + { 0x00, 0x29, 0x04 }, //BL_SETSECTADDR, + { 0x00, 0x28, 0x00 }, //BL_SECT_ERASE, + { 0x00, 0x2C, 0x04 }, //BL_SET_DNLDSIZE, + { 0x00, 0x25, 0x00 }, //BL_DNLD_DATA, + { 0x00, 0x2F, 0x01 }, //BL_FLASH_TYPE, + { 0x00, 0x26, 0x00 }, //BL_CALC_CHKSUM, + { 0x00, 0x30, 0x01 } //BL_PROG_MODE, +}; + +static unsigned char seqNum=0; +static unsigned int PatLut[128] = {0}; +static unsigned int PatLutIndex = 0; + +int LCR_Write() +{ + return USB_Write(); +} + +int LCR_Read() +{ + int ret_val; + hidMessageStruct *pMsg = (hidMessageStruct *)InputBuffer; + if(USB_Write() > 0) + { + ret_val = USB_Read(); + + if((pMsg->head.flags.nack == 1) || (pMsg->head.length == 0)) + return -2; + else + return ret_val; + } + return -1; +} + +int LCR_ContinueRead() +{ + return USB_Read(); +} + +int LCR_SendMsg(hidMessageStruct *pMsg) +{ + int maxDataSize = USB_MAX_PACKET_SIZE-sizeof(pMsg->head); + int dataBytesSent = MIN(pMsg->head.length, maxDataSize); //Send all data or max possible + + OutputBuffer[0]=0; // First byte is the report number + memcpy(&OutputBuffer[1], pMsg, (sizeof(pMsg->head) + dataBytesSent)); + + if(LCR_Write() < 0) + return -1; + + //dataBytesSent = maxDataSize; + + while(dataBytesSent < pMsg->head.length) + { + memcpy(&OutputBuffer[1], &pMsg->text.data[dataBytesSent], USB_MAX_PACKET_SIZE); + if(LCR_Write() < 0) + return -1; + dataBytesSent += USB_MAX_PACKET_SIZE; + } + return dataBytesSent+sizeof(pMsg->head); +} + +int LCR_PrepReadCmd(LCR_CMD cmd) +{ + hidMessageStruct msg; + + msg.head.flags.rw = 1; //Read + msg.head.flags.reply = 1; //Host wants a reply from device + msg.head.flags.dest = 0; //Projector Control Endpoint + msg.head.flags.reserved = 0; + msg.head.flags.nack = 0; + msg.head.seq = 0; + + msg.text.cmd = (CmdList[cmd].CMD2 << 8) | CmdList[cmd].CMD3; + msg.head.length = 2; + + if(cmd == BL_GET_MANID) + { + msg.text.data[2] = 0x0C; + msg.head.length += 1; + } + else if (cmd == BL_GET_DEVID) + { + msg.text.data[2] = 0x0D; + msg.head.length += 1; + } + else if (cmd == BL_GET_CHKSUM) + { + msg.text.data[2] = 0x00; + msg.head.length += 1; + } + + OutputBuffer[0]=0; // First byte is the report number + memcpy(&OutputBuffer[1], &msg, (sizeof(msg.head)+sizeof(msg.text.cmd) + msg.head.length)); + return 0; +} + +int LCR_PrepReadCmdWithParam(LCR_CMD cmd, unsigned char param) +{ + hidMessageStruct msg; + + msg.head.flags.rw = 1; //Read + msg.head.flags.reply = 1; //Host wants a reply from device + msg.head.flags.dest = 0; //Projector Control Endpoint + msg.head.flags.reserved = 0; + msg.head.flags.nack = 0; + msg.head.seq = 0; + + msg.text.cmd = (CmdList[cmd].CMD2 << 8) | CmdList[cmd].CMD3; + msg.head.length = 3; + + msg.text.data[2] = param; + + OutputBuffer[0]=0; // First byte is the report number + memcpy(&OutputBuffer[1], &msg, (sizeof(msg.head)+sizeof(msg.text.cmd) + msg.head.length)); + return 0; +} + +int LCR_PrepMemReadCmd(unsigned int addr) +{ + hidMessageStruct msg; + + msg.head.flags.rw = 1; //Read + msg.head.flags.reply = 1; //Host wants a reply from device + msg.head.flags.dest = 0; //Projector Control Endpoint + msg.head.flags.reserved = 0; + msg.head.flags.nack = 0; + msg.head.seq = 0; + + msg.text.cmd = (CmdList[MEM_CONTROL].CMD2 << 8) | CmdList[MEM_CONTROL].CMD3; + msg.head.length = 6; + + msg.text.data[2] = addr; + msg.text.data[3] = addr >>8; + msg.text.data[4] = addr >>16; + msg.text.data[5] = addr >>24; + + OutputBuffer[0]=0; // First byte is the report number + memcpy(&OutputBuffer[1], &msg, (sizeof(msg.head)+sizeof(msg.text.cmd) + msg.head.length)); + return 0; +} + +int LCR_PrepWriteCmd(hidMessageStruct *pMsg, LCR_CMD cmd) +{ + pMsg->head.flags.rw = 0; //Write + pMsg->head.flags.reply = 0; //Host wants a reply from device + pMsg->head.flags.dest = 0; //Projector Control Endpoint + pMsg->head.flags.reserved = 0; + pMsg->head.flags.nack = 0; + pMsg->head.seq = seqNum++; + + pMsg->text.cmd = (CmdList[cmd].CMD2 << 8) | CmdList[cmd].CMD3; + pMsg->head.length = CmdList[cmd].len + 2; + + return 0; +} + +int LCR_GetVersion(unsigned int *pApp_ver, unsigned int *pAPI_ver, unsigned int *pSWConfig_ver, unsigned int *pSeqConfig_ver) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(GET_VERSION); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pApp_ver = *(unsigned int *)&msg.text.data[0]; + *pAPI_ver = *(unsigned int *)&msg.text.data[4]; + *pSWConfig_ver = *(unsigned int *)&msg.text.data[8]; + *pSeqConfig_ver = *(unsigned int *)&msg.text.data[12]; + return 0; + } + return -1; +} + +int LCR_GetLedEnables(bool *pSeqCtrl, bool *pRed, bool *pGreen, bool *pBlue) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(LED_ENABLE); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + if(msg.text.data[0] & BIT0) + *pRed = true; + else + *pRed = false; + + if(msg.text.data[0] & BIT1) + *pGreen = true; + else + *pGreen = false; + + if(msg.text.data[0] & BIT2) + *pBlue = true; + else + *pBlue = false; + + if(msg.text.data[0] & BIT3) + *pSeqCtrl = true; + else + *pSeqCtrl = false; + return 0; + } + return -1; +} + + +int LCR_SetLedEnables(bool SeqCtrl, bool Red, bool Green, bool Blue) +{ + hidMessageStruct msg; + unsigned char Enable=0; + + if(SeqCtrl) + Enable |= BIT3; + if(Red) + Enable |= BIT0; + if(Green) + Enable |= BIT1; + if(Blue) + Enable |= BIT2; + + msg.text.data[2] = Enable; + LCR_PrepWriteCmd(&msg, LED_ENABLE); + + return LCR_SendMsg(&msg); +} + +int LCR_GetLedCurrents(unsigned char *pRed, unsigned char *pGreen, unsigned char *pBlue) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(LED_CURRENT); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pRed = msg.text.data[0]; + *pGreen = msg.text.data[1]; + *pBlue = msg.text.data[2]; + + return 0; + } + return -1; +} + + +int LCR_SetLedCurrents(unsigned char RedCurrent, unsigned char GreenCurrent, unsigned char BlueCurrent) +{ + hidMessageStruct msg; + + msg.text.data[2] = RedCurrent; + msg.text.data[3] = GreenCurrent; + msg.text.data[4] = BlueCurrent; + + LCR_PrepWriteCmd(&msg, LED_CURRENT); + + return LCR_SendMsg(&msg); +} + +bool LCR_GetLongAxisImageFlip(void) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(FLIP_LONG); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + if ((msg.text.data[0] & BIT0) == BIT0) + return true; + else + return false; + } + return false; +} + +bool LCR_GetShortAxisImageFlip(void) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(FLIP_SHORT); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + if ((msg.text.data[0] & BIT0) == BIT0) + return true; + else + return false; + } + return false; +} + + +int LCR_SetLongAxisImageFlip(bool Flip) +{ + hidMessageStruct msg; + + if(Flip) + msg.text.data[2] = BIT0; + else + msg.text.data[2] = 0; + + LCR_PrepWriteCmd(&msg, FLIP_LONG); + + return LCR_SendMsg(&msg); +} + +int LCR_SetShortAxisImageFlip(bool Flip) +{ + hidMessageStruct msg; + + if(Flip) + msg.text.data[2] = BIT0; + else + msg.text.data[2] = 0; + + LCR_PrepWriteCmd(&msg, FLIP_SHORT); + + return LCR_SendMsg(&msg); +} + +int LCR_SetProgrammingMode(bool EnterProgMode) +{ + hidMessageStruct msg; + + if(EnterProgMode) + msg.text.data[2] = 1; + else + msg.text.data[2] = 2; + + LCR_PrepWriteCmd(&msg, PROG_MODE); + + return LCR_SendMsg(&msg); +} + +int LCR_ExitProgrammingMode(void) +{ + hidMessageStruct msg; + + msg.text.data[2] = 2; + LCR_PrepWriteCmd(&msg, BL_PROG_MODE); + + return LCR_SendMsg(&msg); +} + + +int LCR_GetProgrammingMode(bool *ProgMode) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(PROG_MODE); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + if(msg.text.data[0] == 1) + *ProgMode = true; + else + *ProgMode = false; + return 0; + } + return -1; +} + +int LCR_GetFlashManID(unsigned short *pManID) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(BL_GET_MANID); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pManID = msg.text.data[6]; + *pManID |= (unsigned short)msg.text.data[7] << 8; + return 0; + } + return -1; +} + +int LCR_GetFlashDevID(unsigned long long *pDevID) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(BL_GET_DEVID); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pDevID = msg.text.data[6]; + *pDevID |= (unsigned long long)msg.text.data[7] << 8; + *pDevID |= (unsigned long long)msg.text.data[8] << 16; + *pDevID |= (unsigned long long)msg.text.data[9] << 24; + *pDevID |= (unsigned long long)msg.text.data[12] << 32; + *pDevID |= (unsigned long long)msg.text.data[13] << 40; + *pDevID |= (unsigned long long)msg.text.data[14] << 48; + *pDevID |= (unsigned long long)msg.text.data[15] << 56; + return 0; + } + return -1; +} + +int LCR_GetBLStatus(unsigned char *BL_Status) +{ + hidMessageStruct msg; + + /* For some reason BL_STATUS readback is not working properly. + * However, after going through the bootloader code, I have ascertained that any + * readback is fine - Byte 0 is always teh bootloader status */ + LCR_PrepReadCmd(BL_GET_CHKSUM); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *BL_Status = msg.text.data[0]; + return 0; + } + return -1; +} + +int LCR_BLSpecialMode(unsigned int Mode) +{ + hidMessageStruct msg; + + msg.text.data[2] = Mode; + + LCR_PrepWriteCmd(&msg, BL_SPL_MODE); + + return LCR_SendMsg(&msg); +} + +int LCR_SetFlashAddr(unsigned int Addr) +{ + hidMessageStruct msg; + + msg.text.data[2] = Addr; + msg.text.data[3] = Addr >> 8; + msg.text.data[4] = Addr >> 16; + msg.text.data[5] = Addr >> 24; + + LCR_PrepWriteCmd(&msg, BL_SET_SECTADDR); + + return LCR_SendMsg(&msg); +} + + int LCR_FlashSectorErase(void) + { + hidMessageStruct msg; + + LCR_PrepWriteCmd(&msg, BL_SECT_ERASE); + return LCR_SendMsg(&msg); + } + +int LCR_SetDownloadSize(unsigned int dataLen) +{ + hidMessageStruct msg; + + msg.text.data[2] = dataLen; + msg.text.data[3] = dataLen >> 8; + msg.text.data[4] = dataLen >> 16; + msg.text.data[5] = dataLen >> 24; + + LCR_PrepWriteCmd(&msg, BL_SET_DNLDSIZE); + + return LCR_SendMsg(&msg); +} + +int LCR_DownloadData(unsigned char *pByteArray, unsigned int dataLen) +{ + hidMessageStruct msg; + int retval; + unsigned int sendSize; + + sendSize = HID_MESSAGE_MAX_SIZE - sizeof(msg.head)- sizeof(msg.text.cmd) - 2;//The last -2 is to workaround a bug in bootloader. + + if(dataLen > sendSize) + dataLen = sendSize; + + CmdList[BL_DNLD_DATA].len = dataLen; + memcpy(&msg.text.data[2], pByteArray, dataLen); + + LCR_PrepWriteCmd(&msg, BL_DNLD_DATA); + + retval = LCR_SendMsg(&msg); + if(retval > 0) + return dataLen; + + return -1; +} + +void LCR_WaitForFlashReady() +{ + unsigned char BLstatus=STAT_BIT_FLASH_BUSY; + + do + { + LCR_GetBLStatus(&BLstatus); + } + while((BLstatus & STAT_BIT_FLASH_BUSY) == STAT_BIT_FLASH_BUSY);//Wait for flash busy flag to go off +} + +int LCR_SetFlashType(unsigned char Type) +{ + hidMessageStruct msg; + + msg.text.data[2] = Type; + + LCR_PrepWriteCmd(&msg, BL_FLASH_TYPE); + + return LCR_SendMsg(&msg); +} + +int LCR_CalculateFlashChecksum(void) +{ + hidMessageStruct msg; + + LCR_PrepWriteCmd(&msg, BL_CALC_CHKSUM); + + if(LCR_SendMsg(&msg) <= 0) + return -1; + + return 0; + +} + +int LCR_GetFlashChecksum(unsigned int*checksum) +{ + hidMessageStruct msg; +#if 0 + LCR_PrepWriteCmd(&msg, BL_CALC_CHKSUM); + if(LCR_SendMsg(&msg) <= 0) + return -1; + LCR_WaitForFlashReady(); +#endif + LCR_PrepReadCmd(BL_GET_CHKSUM); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *checksum = msg.text.data[6]; + *checksum |= (unsigned int)msg.text.data[7] << 8; + *checksum |= (unsigned int)msg.text.data[8] << 16; + *checksum |= (unsigned int)msg.text.data[9] << 24; + return 0; + } + return -1; +} + +int LCR_GetStatus(unsigned char *pHWStatus, unsigned char *pSysStatus, unsigned char *pMainStatus) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(STATUS_HW); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pHWStatus = msg.text.data[0]; + } + else + return -1; + + LCR_PrepReadCmd(STATUS_SYS); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pSysStatus = msg.text.data[0]; + } + else + return -1; + + LCR_PrepReadCmd(STATUS_MAIN); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + + *pMainStatus = msg.text.data[0]; + } + else + return -1; + + return 0; +} + +int LCR_SoftwareReset(void) +{ + hidMessageStruct msg; + + LCR_PrepWriteCmd(&msg, SW_RESET); + + return LCR_SendMsg(&msg); +} + +int LCR_SetMode(bool SLmode) +{ + hidMessageStruct msg; + + msg.text.data[2] = SLmode; + LCR_PrepWriteCmd(&msg, DISP_MODE); + + return LCR_SendMsg(&msg); +} + +int LCR_GetMode(bool *pMode) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(DISP_MODE); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pMode = (msg.text.data[0] != 0); + return 0; + } + return -1; +} + +int LCR_SetPowerMode(bool Standby) +{ + hidMessageStruct msg; + + msg.text.data[2] = Standby; + LCR_PrepWriteCmd(&msg, POWER_CONTROL); + + return LCR_SendMsg(&msg); +} + +int LCR_SetRedLEDStrobeDelay(unsigned char rising, unsigned char falling) +{ + hidMessageStruct msg; + + msg.text.data[2] = rising; + msg.text.data[3] = falling; + + LCR_PrepWriteCmd(&msg, RED_STROBE_DLY); + + return LCR_SendMsg(&msg); +} + +int LCR_SetGreenLEDStrobeDelay(unsigned char rising, unsigned char falling) +{ + hidMessageStruct msg; + + msg.text.data[2] = rising; + msg.text.data[3] = falling; + + LCR_PrepWriteCmd(&msg, GRN_STROBE_DLY); + + return LCR_SendMsg(&msg); +} + +int LCR_SetBlueLEDStrobeDelay(unsigned char rising, unsigned char falling) +{ + hidMessageStruct msg; + + msg.text.data[2] = rising; + msg.text.data[3] = falling; + + LCR_PrepWriteCmd(&msg, BLU_STROBE_DLY); + + return LCR_SendMsg(&msg); +} + +int LCR_GetRedLEDStrobeDelay(unsigned char *pRising, unsigned char *pFalling) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(RED_STROBE_DLY); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pRising = msg.text.data[0]; + *pFalling = msg.text.data[1]; + return 0; + } + return -1; +} + +int LCR_GetGreenLEDStrobeDelay(unsigned char *pRising, unsigned char *pFalling) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(GRN_STROBE_DLY); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pRising = msg.text.data[0]; + *pFalling = msg.text.data[1]; + return 0; + } + return -1; +} + +int LCR_GetBlueLEDStrobeDelay(unsigned char *pRising, unsigned char *pFalling) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(BLU_STROBE_DLY); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pRising = msg.text.data[0]; + *pFalling = msg.text.data[1]; + return 0; + } + return -1; +} + +int LCR_SetInputSource(unsigned int source, unsigned int portWidth) +{ + hidMessageStruct msg; + + msg.text.data[2] = source; + msg.text.data[2] |= portWidth << 3; + LCR_PrepWriteCmd(&msg, SOURCE_SEL); + + return LCR_SendMsg(&msg); +} + +int LCR_GetInputSource(unsigned int *pSource, unsigned int *pPortWidth) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(SOURCE_SEL); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pSource = msg.text.data[0] & (BIT0 | BIT1 | BIT2); + *pPortWidth = msg.text.data[0] >> 3; + return 0; + } + return -1; +} + +int LCR_SetPatternDisplayMode(bool external) +{ + hidMessageStruct msg; + + if(external) + msg.text.data[2] = 0; + else + msg.text.data[2] = 3; + + LCR_PrepWriteCmd(&msg, PAT_DISP_MODE); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPatternDisplayMode(bool *external) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(PAT_DISP_MODE); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + if(msg.text.data[0] == 0) + *external = true; + else + *external = false; + return 0; + } + return -1; +} + +int LCR_SetPixelFormat(unsigned int format) +{ + hidMessageStruct msg; + + msg.text.data[2] = format; + LCR_PrepWriteCmd(&msg, PIXEL_FORMAT); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPixelFormat(unsigned int *pFormat) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(PIXEL_FORMAT); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pFormat = msg.text.data[0] & (BIT0 | BIT1 | BIT2); + return 0; + } + return -1; +} + +int LCR_SetPortClock(unsigned int clock) +{ + hidMessageStruct msg; + + msg.text.data[2] = clock; + LCR_PrepWriteCmd(&msg, CLK_SEL); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPortClock(unsigned int *pClock) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(CLK_SEL); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pClock = msg.text.data[0] & (BIT0 | BIT1 | BIT2); + return 0; + } + return -1; +} + +int LCR_SetDataChannelSwap(unsigned int port, unsigned int swap) +{ + hidMessageStruct msg; + + msg.text.data[2] = port << 7; + msg.text.data[2] |= swap & (BIT0 | BIT1 | BIT2); + LCR_PrepWriteCmd(&msg, CHANNEL_SWAP); + + return LCR_SendMsg(&msg); +} + +int LCR_GetDataChannelSwap(unsigned int *pPort, unsigned int *pSwap) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(CHANNEL_SWAP); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pSwap = msg.text.data[0] & (BIT0 | BIT1 | BIT2); + if(msg.text.data[0] & BIT7) + *pPort = 1; + else + *pPort = 0; + return 0; + } + return -1; +} + +int LCR_SetFPD_Mode_Field(unsigned int PixelMappingMode, bool SwapPolarity, unsigned int FieldSignalSelect) +{ + hidMessageStruct msg; + + msg.text.data[2] = PixelMappingMode << 6; + msg.text.data[2] |= FieldSignalSelect & (BIT0 | BIT1 | BIT2); + if(SwapPolarity) + msg.text.data[2] |= BIT3; + LCR_PrepWriteCmd(&msg, FPD_MODE); + + return LCR_SendMsg(&msg); +} + +int LCR_GetFPD_Mode_Field(unsigned int *pPixelMappingMode, bool *pSwapPolarity, unsigned int *pFieldSignalSelect) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(FPD_MODE); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pFieldSignalSelect = msg.text.data[0] & (BIT0 | BIT1 | BIT2); + if(msg.text.data[0] & BIT3) + *pSwapPolarity = 1; + else + *pSwapPolarity = 0; + *pPixelMappingMode = msg.text.data[0] >> 6; + return 0; + } + return -1; +} + +int LCR_SetTPGSelect(unsigned int pattern) +{ + hidMessageStruct msg; + + msg.text.data[2] = pattern; + LCR_PrepWriteCmd(&msg, TPG_SEL); + + return LCR_SendMsg(&msg); +} + +int LCR_GetTPGSelect(unsigned int *pPattern) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(TPG_SEL); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pPattern = msg.text.data[0] & (BIT0 | BIT1 | BIT2 | BIT3); + return 0; + } + return -1; +} + +int LCR_LoadSplash(unsigned int index) +{ + hidMessageStruct msg; + + msg.text.data[2] = index; + LCR_PrepWriteCmd(&msg, SPLASH_LOAD); + + return LCR_SendMsg(&msg); +} + +int LCR_GetSplashIndex(unsigned int *pIndex) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(SPLASH_LOAD); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pIndex = msg.text.data[0]; + return 0; + } + return -1; +} + +int LCR_SetDisplay(rectangle croppedArea, rectangle displayArea) +{ + hidMessageStruct msg; + + msg.text.data[2] = croppedArea.firstPixel & 0xFF; + msg.text.data[3] = croppedArea.firstPixel >> 8; + msg.text.data[4] = croppedArea.firstLine & 0xFF; + msg.text.data[5] = croppedArea.firstLine >> 8; + msg.text.data[6] = croppedArea.pixelsPerLine & 0xFF; + msg.text.data[7] = croppedArea.pixelsPerLine >> 8; + msg.text.data[8] = croppedArea.linesPerFrame & 0xFF; + msg.text.data[9] = croppedArea.linesPerFrame >> 8; + msg.text.data[10] = displayArea.firstPixel & 0xFF; + msg.text.data[11] = displayArea.firstPixel >> 8; + msg.text.data[12] = displayArea.firstLine & 0xFF; + msg.text.data[13] = displayArea.firstLine >> 8; + msg.text.data[14] = displayArea.pixelsPerLine & 0xFF; + msg.text.data[15] = displayArea.pixelsPerLine >> 8; + msg.text.data[16] = displayArea.linesPerFrame & 0xFF; + msg.text.data[17] = displayArea.linesPerFrame >> 8; + + LCR_PrepWriteCmd(&msg, DISP_CONFIG); + + return LCR_SendMsg(&msg); +} + +int LCR_GetDisplay(rectangle *pCroppedArea, rectangle *pDisplayArea) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(DISP_CONFIG); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + pCroppedArea->firstPixel = msg.text.data[0] | msg.text.data[1] << 8; + pCroppedArea->firstLine = msg.text.data[2] | msg.text.data[3] << 8; + pCroppedArea->pixelsPerLine = msg.text.data[4] | msg.text.data[5] << 8; + pCroppedArea->linesPerFrame = msg.text.data[6] | msg.text.data[7] << 8; + pDisplayArea->firstPixel = msg.text.data[8] | msg.text.data[9] << 8; + pDisplayArea->firstLine = msg.text.data[10] | msg.text.data[11] << 8; + pDisplayArea->pixelsPerLine = msg.text.data[12] | msg.text.data[13] << 8; + pDisplayArea->linesPerFrame = msg.text.data[14] | msg.text.data[15] << 8; + + return 0; + } + return -1; +} + +int LCR_SetTPGColor(unsigned short redFG, unsigned short greenFG, unsigned short blueFG, unsigned short redBG, unsigned short greenBG, unsigned short blueBG) +{ + hidMessageStruct msg; + + msg.text.data[2] = (char)redFG; + msg.text.data[3] = (char)(redFG >> 8); + msg.text.data[4] = (char)greenFG; + msg.text.data[5] = (char)(greenFG >> 8); + msg.text.data[6] = (char)blueFG; + msg.text.data[7] = (char)(blueFG >> 8); + msg.text.data[8] = (char)redBG; + msg.text.data[9] = (char)(redBG >> 8); + msg.text.data[10] = (char)greenBG; + msg.text.data[11] = (char)(greenBG >> 8); + msg.text.data[12] = (char)blueBG; + msg.text.data[13] = (char)(blueBG >> 8); + + LCR_PrepWriteCmd(&msg, TPG_COLOR); + + return LCR_SendMsg(&msg); +} + +int LCR_GetTPGColor(unsigned short *pRedFG, unsigned short *pGreenFG, unsigned short *pBlueFG, unsigned short *pRedBG, unsigned short *pGreenBG, unsigned short *pBlueBG) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(TPG_COLOR); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pRedFG = msg.text.data[0] | msg.text.data[1] << 8; + *pGreenFG = msg.text.data[2] | msg.text.data[3] << 8; + *pBlueFG = msg.text.data[4] | msg.text.data[5] << 8; + *pRedBG = msg.text.data[6] | msg.text.data[7] << 8; + *pGreenBG = msg.text.data[8] | msg.text.data[9] << 8; + *pBlueBG = msg.text.data[10] | msg.text.data[11] << 8; + + return 0; + } + return -1; +} + +int LCR_ClearPatLut(void) +{ + PatLutIndex = 0; + return 0; +} + +int LCR_AddToPatLut(int TrigType, int PatNum,int BitDepth,int LEDSelect,bool InvertPat, bool InsertBlack,bool BufSwap, bool trigOutPrev) +{ + unsigned int lutWord = 0; + + lutWord = TrigType & 3; + if(PatNum > 24) + return -1; + + lutWord |= ((PatNum & 0x3F) << 2); + if( (BitDepth > 8) || (BitDepth <= 0)) + return -1; + lutWord |= ((BitDepth & 0xF) << 8); + if(LEDSelect > 7) + return -1; + lutWord |= ((LEDSelect & 0x7) << 12); + if(InvertPat) + lutWord |= BIT16; + if(InsertBlack) + lutWord |= BIT17; + if(BufSwap) + lutWord |= BIT18; + if(trigOutPrev) + lutWord |= BIT19; + + PatLut[PatLutIndex++] = lutWord; + return 0; +} + +int LCR_GetPatLutItem(int index, int *pTrigType, int *pPatNum,int *pBitDepth,int *pLEDSelect,bool *pInvertPat, bool *pInsertBlack,bool *pBufSwap, bool *pTrigOutPrev) +{ + unsigned int lutWord; + + lutWord = PatLut[index]; + + *pTrigType = lutWord & 3; + *pPatNum = (lutWord >> 2) & 0x3F; + *pBitDepth = (lutWord >> 8) & 0xF; + *pLEDSelect = (lutWord >> 12) & 7; + *pInvertPat = ((lutWord & BIT16) == BIT16); + *pInsertBlack = ((lutWord & BIT17) == BIT17); + *pBufSwap = ((lutWord & BIT18) == BIT18); + *pTrigOutPrev = ((lutWord & BIT19) == BIT19); + + return 0; +} + +int LCR_OpenMailbox(int MboxNum) +{ + hidMessageStruct msg; + + msg.text.data[2] = MboxNum; + LCR_PrepWriteCmd(&msg, MBOX_CONTROL); + + return LCR_SendMsg(&msg); +} + +int LCR_CloseMailbox(void) +{ + hidMessageStruct msg; + + msg.text.data[2] = 0; + LCR_PrepWriteCmd(&msg, MBOX_CONTROL); + + return LCR_SendMsg(&msg); +} + +int LCR_MailboxSetAddr(int Addr) +{ + hidMessageStruct msg; + + if(Addr > 127) + return -1; + + msg.text.data[2] = Addr; + LCR_PrepWriteCmd(&msg, MBOX_ADDRESS); + + return LCR_SendMsg(&msg); +} + +int LCR_SendPatLut(void) +{ + hidMessageStruct msg; + int bytesToSend=PatLutIndex*3; + unsigned int i; + + if(LCR_OpenMailbox(2) < 0) + return -1; + LCR_MailboxSetAddr(0); + + CmdList[MBOX_DATA].len = bytesToSend; + LCR_PrepWriteCmd(&msg, MBOX_DATA); + + for(i=0; i>8; + msg.text.data[2+3*i+2] = PatLut[i]>>16; + } + + LCR_SendMsg(&msg); + LCR_CloseMailbox(); + return 0; +} + +int LCR_SendSplashLut(unsigned char *lutEntries, unsigned int numEntries) +{ + hidMessageStruct msg; + unsigned int i; + + if(numEntries < 1 || numEntries > 64) + return -1; + + LCR_OpenMailbox(1); + LCR_MailboxSetAddr(0); + + // Check for special case of 2 entries + if( numEntries == 2) + { + msg.text.data[2+0] = lutEntries[1]; + msg.text.data[2+1] = lutEntries[0]; + } + else + { + for(i=0; i < numEntries; i++) + { + msg.text.data[2+i] = lutEntries[i]; + } + } + + CmdList[MBOX_DATA].len = numEntries; + LCR_PrepWriteCmd(&msg, MBOX_DATA); + LCR_SendMsg(&msg); + LCR_CloseMailbox(); + + return 0; +} + + +int LCR_GetPatLut(int numEntries) +{ + hidMessageStruct msg; + unsigned int lutWord = 0; + int numBytes, i; + unsigned char *readBuf; + + if(numEntries > 128) + return -1; + + if(LCR_OpenMailbox(2) < 0) + return -1; + + if(LCR_MailboxSetAddr(0) < 0) + return -1; + + numBytes = sizeof(msg.head)+numEntries*3; + readBuf = (unsigned char *)&msg; + LCR_PrepReadCmd(MBOX_DATA); + + + if(LCR_Read() > 0) + { + memcpy(readBuf, InputBuffer, MIN(numBytes,64)); + readBuf+=64; + numBytes -=64; + } + else + { + LCR_CloseMailbox(); + return -1; + } + /* If packet is greater than 64 bytes, continue to read */ + while(numBytes > 0) + { + LCR_ContinueRead(); + memcpy(readBuf, InputBuffer, MIN(numBytes,64)); + readBuf+=64; + numBytes -=64; + } + + LCR_ClearPatLut(); + for(i=0; i 0) + { + memcpy(pLut, InputBuffer+sizeof(pMsg->head), MIN(numEntries,64-sizeof(pMsg->head))); + pLut+= (64-sizeof(pMsg->head)); + numEntries -= (64-sizeof(pMsg->head)); + } + else + { + LCR_CloseMailbox(); + return -1; + } + + /* If packet is greater than 64 bytes, continue to read */ + while(numEntries > 0) + { + LCR_ContinueRead(); + memcpy(pLut, InputBuffer, MIN(numEntries,64)); + pLut+=64; + numEntries -= 64; + } + + if(LCR_CloseMailbox() < 0) + return -1; + + return 0; +} + +int LCR_SetPatternTriggerMode(bool Vsync_or_Generated) +{ + hidMessageStruct msg; + + msg.text.data[2] = Vsync_or_Generated; + LCR_PrepWriteCmd(&msg, PAT_TRIG_MODE); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPatternTriggerMode(bool *Vsync_or_Generated) +{ hidMessageStruct msg; + + LCR_PrepReadCmd(PAT_TRIG_MODE); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *Vsync_or_Generated = (msg.text.data[0] != 0); + return 0; + } + return -1; +} + + +int LCR_PatternDisplay(int Action) +{ + hidMessageStruct msg; + + msg.text.data[2] = Action; + LCR_PrepWriteCmd(&msg, PAT_START_STOP); + + return LCR_SendMsg(&msg); +} + +int LCR_SetPatternConfig(unsigned int numLutEntries, bool repeat, unsigned int numPatsForTrigOut2, unsigned int numSplash) +{ + hidMessageStruct msg; + + msg.text.data[2] = numLutEntries-1; /* -1 because the firmware command takes 0-based indices (0 means 1) */ + msg.text.data[3] = repeat; + msg.text.data[4] = numPatsForTrigOut2 - 1; /* -1 because the firmware command takes 0-based indices (0 means 1) */ + msg.text.data[5] = numSplash - 1; /* -1 because the firmware command takes 0-based indices (0 means 1) */ + LCR_PrepWriteCmd(&msg, PAT_CONFIG); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPatternConfig(unsigned int *pNumLutEntries, bool *pRepeat, unsigned int *pNumPatsForTrigOut2, unsigned int *pNumSplash) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(PAT_CONFIG); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pNumLutEntries = msg.text.data[0] + 1; /* +1 because the firmware gives 0-based indices (0 means 1) */ + *pRepeat = (msg.text.data[1] != 0); + *pNumPatsForTrigOut2 = msg.text.data[2]+1; /* +1 because the firmware gives 0-based indices (0 means 1) */ + *pNumSplash = msg.text.data[3]+1; /* +1 because the firmware gives 0-based indices (0 means 1) */ + return 0; + } + return -1; +} + +int LCR_SetExpsosure_FramePeriod(unsigned int exposurePeriod, unsigned int framePeriod) +{ + hidMessageStruct msg; + + msg.text.data[2] = exposurePeriod; + msg.text.data[3] = exposurePeriod>>8; + msg.text.data[4] = exposurePeriod>>16; + msg.text.data[5] = exposurePeriod>>24; + msg.text.data[6] = framePeriod; + msg.text.data[7] = framePeriod>>8; + msg.text.data[8] = framePeriod>>16; + msg.text.data[9] = framePeriod>>24; + LCR_PrepWriteCmd(&msg, PAT_EXPO_PRD); + + return LCR_SendMsg(&msg); +} + +int LCR_GetExposure_FramePeriod(unsigned int *pExposure, unsigned int *pFramePeriod) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(PAT_EXPO_PRD); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pExposure = msg.text.data[0] | msg.text.data[1] << 8 | msg.text.data[2] << 16 | msg.text.data[3] << 24; + *pFramePeriod = msg.text.data[4] | msg.text.data[5] << 8 | msg.text.data[6] << 16 | msg.text.data[7] << 24; + return 0; + } + return -1; +} + + +int LCR_SetTrigOutConfig(unsigned int trigOutNum, bool invert, unsigned int rising, unsigned int falling) +{ + hidMessageStruct msg; + + msg.text.data[2] = invert; + msg.text.data[3] = rising; + msg.text.data[4] = falling; + if(trigOutNum == 1) + LCR_PrepWriteCmd(&msg, TRIG_OUT1_CTL); + else if(trigOutNum==2) + LCR_PrepWriteCmd(&msg, TRIG_OUT2_CTL); + + return LCR_SendMsg(&msg); +} + +int LCR_GetTrigOutConfig(unsigned int trigOutNum, bool *pInvert,unsigned int *pRising, unsigned int *pFalling) +{ + hidMessageStruct msg; + + if(trigOutNum == 1) + LCR_PrepReadCmd(TRIG_OUT1_CTL); + else if(trigOutNum==2) + LCR_PrepReadCmd(TRIG_OUT2_CTL); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pInvert = (msg.text.data[0] != 0); + *pRising = msg.text.data[1]; + *pFalling = msg.text.data[2]; + return 0; + } + return -1; +} + +int LCR_ValidatePatLutData(unsigned int *pStatus) +{ + hidMessageStruct msg; + + LCR_PrepWriteCmd(&msg, LUT_VALID); + if(LCR_SendMsg(&msg) < 0) + return -1; + + LCR_PrepReadCmd(LUT_VALID); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pStatus = msg.text.data[0]; + return 0; + } + return -1; +} + + +int LCR_SetTrigIn1Delay(unsigned int Delay) +{ + hidMessageStruct msg; + + msg.text.data[2] = Delay; + msg.text.data[3] = Delay >> 8; + msg.text.data[4] = Delay >> 16; + msg.text.data[5] = Delay >> 24; + LCR_PrepWriteCmd(&msg, TRIG_IN1_DELAY); + + return LCR_SendMsg(&msg); +} + +int LCR_GetTrigIn1Delay(unsigned int *pDelay) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(TRIG_IN1_DELAY); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pDelay = msg.text.data[0] | msg.text.data[1]<<8 | msg.text.data[2]<<16 | msg.text.data[3]<<24; + return 0; + } + return -1; +} + +int LCR_SetInvertData(bool invert) +{ + hidMessageStruct msg; + + msg.text.data[2] = invert; + LCR_PrepWriteCmd(&msg, INVERT_DATA); + + return LCR_SendMsg(&msg); +} + +int LCR_SetPWMConfig(unsigned int channel, unsigned int pulsePeriod, unsigned int dutyCycle) +{ + hidMessageStruct msg; + + msg.text.data[2] = channel; + msg.text.data[3] = pulsePeriod; + msg.text.data[4] = pulsePeriod >> 8; + msg.text.data[5] = pulsePeriod >> 16; + msg.text.data[6] = pulsePeriod >> 24; + msg.text.data[7] = dutyCycle; + + LCR_PrepWriteCmd(&msg, PWM_SETUP); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPWMConfig(unsigned int channel, unsigned int *pPulsePeriod, unsigned int *pDutyCycle) +{ + hidMessageStruct msg; + + LCR_PrepReadCmdWithParam(PWM_SETUP, (unsigned char)channel); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pPulsePeriod = msg.text.data[1] | msg.text.data[2] << 8 | msg.text.data[3] << 16 | msg.text.data[4] << 24; + *pDutyCycle = msg.text.data[5]; + return 0; + } + return -1; +} + +int LCR_SetPWMEnable(unsigned int channel, bool Enable) +{ + hidMessageStruct msg; + unsigned char value = 0; + + if(Enable) + value = BIT7; + + if(channel == 2) + value |= 2; + else if (channel != 0) + return -1; + + msg.text.data[2] = value; + LCR_PrepWriteCmd(&msg, PWM_ENABLE); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPWMEnable(unsigned int channel, bool *pEnable) +{ + hidMessageStruct msg; + + LCR_PrepReadCmdWithParam(PWM_ENABLE, (unsigned char)channel); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + if(msg.text.data[0] & BIT7) + *pEnable = true; + else + *pEnable = false; + + return 0; + } + return -1; +} + +int LCR_SetPWMCaptureConfig(unsigned int channel, bool enable, unsigned int sampleRate) +{ + hidMessageStruct msg; + unsigned char value = 0; + + value = channel & 1; + + if(enable) + value |= BIT7; + + msg.text.data[2] = value; + msg.text.data[3] = sampleRate; + msg.text.data[4] = sampleRate >> 8; + msg.text.data[5] = sampleRate >> 16; + msg.text.data[6] = sampleRate >> 24; + LCR_PrepWriteCmd(&msg, PWM_CAPTURE_CONFIG); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPWMCaptureConfig(unsigned int channel, bool *pEnabled, unsigned int *pSampleRate) +{ + hidMessageStruct msg; + + LCR_PrepReadCmdWithParam(PWM_CAPTURE_CONFIG, (unsigned char)channel); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + if(msg.text.data[0] & BIT7) + *pEnabled = true; + else + *pEnabled = false; + + *pSampleRate = msg.text.data[1] | msg.text.data[2] << 8 | msg.text.data[3] << 16 | msg.text.data[4] << 24; + + return 0; + } + return -1; +} + +int LCR_PWMCaptureRead(unsigned int channel, unsigned int *pLowPeriod, unsigned int *pHighPeriod) +{ + hidMessageStruct msg; + + LCR_PrepReadCmdWithParam(PWM_CAPTURE_READ, (unsigned char)channel); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pLowPeriod = msg.text.data[1] | msg.text.data[2] << 8; + *pHighPeriod = msg.text.data[3] | msg.text.data[4] << 8; + return 0; + } + return -1; +} + +int LCR_SetGPIOConfig(unsigned int pinNum, bool enAltFunc, bool altFunc1, bool dirOutput, bool outTypeOpenDrain, bool pinState) +{ + hidMessageStruct msg; + unsigned char value = 0; + + if(enAltFunc) + value |= BIT7; + if(altFunc1) + value |= BIT6; + if(dirOutput) + value |= BIT5; + if(outTypeOpenDrain) + value |= BIT4; + if(pinState) + value |= BIT3; + + msg.text.data[2] = pinNum; + msg.text.data[3] = value; + LCR_PrepWriteCmd(&msg, GPIO_CONFIG); + + return LCR_SendMsg(&msg); +} + +int LCR_GetGPIOConfig(unsigned int pinNum, bool *pEnAltFunc, bool *pAltFunc1, bool *pDirOutput, bool *pOutTypeOpenDrain, bool *pState) +{ + hidMessageStruct msg; + + LCR_PrepReadCmdWithParam(GPIO_CONFIG, (unsigned char)pinNum); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pEnAltFunc = ((msg.text.data[1] & BIT7) == BIT7); + *pAltFunc1 = ((msg.text.data[1] & BIT6) == BIT6); + *pDirOutput = ((msg.text.data[1] & BIT5) == BIT5); + *pOutTypeOpenDrain = ((msg.text.data[1] & BIT4) == BIT4); + if(*pDirOutput) + *pState = ((msg.text.data[1] & BIT3) == BIT3); + else + *pState = ((msg.text.data[1] & BIT2) == BIT2); + return 0; + } + return -1; +} + +int LCR_SetGeneralPurposeClockOutFreq(unsigned int clkId, bool enable, unsigned int clkDivider) +{ + hidMessageStruct msg; + + msg.text.data[2] = clkId; + msg.text.data[3] = enable; + msg.text.data[4] = clkDivider; + LCR_PrepWriteCmd(&msg, GPCLK_CONFIG); + + return LCR_SendMsg(&msg); +} + +int LCR_GetGeneralPurposeClockOutFreq(unsigned int clkId, bool *pEnabled, unsigned int *pClkDivider) +{ + hidMessageStruct msg; + + LCR_PrepReadCmdWithParam(GPCLK_CONFIG, (unsigned char)clkId); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *pEnabled = (msg.text.data[0] != 0); + *pClkDivider = msg.text.data[1]; + return 0; + } + return -1; +} + +int LCR_SetPWMInvert(bool invert) +{ + hidMessageStruct msg; + + msg.text.data[2] = invert; + LCR_PrepWriteCmd(&msg, PWM_INVERT); + + return LCR_SendMsg(&msg); +} + +int LCR_GetPWMInvert(bool *inverted) +{ + hidMessageStruct msg; + + LCR_PrepReadCmd(PWM_INVERT); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *inverted = (msg.text.data[0] != 0); + return 0; + } + return -1; +} + +int LCR_MemRead(unsigned int addr, unsigned int *readWord) +{ + hidMessageStruct msg; + + LCR_PrepMemReadCmd(addr); + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + *readWord = msg.text.data[0] | msg.text.data[1] << 8 | msg.text.data[2] << 16 | msg.text.data[3] << 24; + //*readWord = msg.text.data[3] | msg.text.data[2] << 8 | msg.text.data[1] << 16 | msg.text.data[0] << 24; //MSB first + return 0; + } + return -1; +} + +int LCR_MemWrite(unsigned int addr, unsigned int data) +{ + hidMessageStruct msg; + + msg.text.data[2] = 0; //absolute write + msg.text.data[3] = addr >> 24; //MSB first + msg.text.data[4] = addr >> 16; + msg.text.data[5] = addr >> 8; + msg.text.data[6] = addr; + msg.text.data[7] = data >> 24; //MSB first + msg.text.data[8] = data >> 16; + msg.text.data[9] = data >> 8; + msg.text.data[10] = data; + LCR_PrepWriteCmd(&msg, MEM_CONTROL); + + return LCR_SendMsg(&msg); +} + + int LCR_MeasureSplashLoadTiming(unsigned int startIndex, unsigned int numSplash) + { + hidMessageStruct msg; + + msg.text.data[2] = startIndex; + msg.text.data[3] = numSplash; + LCR_PrepWriteCmd(&msg, SPLASH_LOAD_TIMING); + + return LCR_SendMsg(&msg); + } + + int LCR_ReadSplashLoadTiming(unsigned int *pTimingData) + { + hidMessageStruct msg; + int i; + + LCR_PrepReadCmd(SPLASH_LOAD_TIMING); + + if(LCR_Read() > 0) + { + memcpy(&msg, InputBuffer, 65); + for(i=0; i<12; i++) + { + *(pTimingData+i) = (msg.text.data[4*i+0] | msg.text.data[4*i+1] << 8 | msg.text.data[4*i+2] << 16 | msg.text.data[4*i+3] << 24); + } + return 0; + } + return -1; + } diff --git a/Classes/API.h b/Classes/API.h new file mode 100644 index 0000000..872730a --- /dev/null +++ b/Classes/API.h @@ -0,0 +1,259 @@ +/* + * API.h + * + * This module provides C callable APIs for each of the command supported by LightCrafter4500 platform and detailed in the programmer's guide. + * + * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/ + * ALL RIGHTS RESERVED + * +*/ + +#ifndef API_H +#define API_H + +/* Bit masks. */ +#define BIT0 0x01 +#define BIT1 0x02 +#define BIT2 0x04 +#define BIT3 0x08 +#define BIT4 0x10 +#define BIT5 0x20 +#define BIT6 0x40 +#define BIT7 0x80 +#define BIT8 0x0100 +#define BIT9 0x0200 +#define BIT10 0x0400 +#define BIT11 0x0800 +#define BIT12 0x1000 +#define BIT13 0x2000 +#define BIT14 0x4000 +#define BIT15 0x8000 +#define BIT16 0x00010000 +#define BIT17 0x00020000 +#define BIT18 0x00040000 +#define BIT19 0x00080000 +#define BIT20 0x00100000 +#define BIT21 0x00200000 +#define BIT22 0x00400000 +#define BIT23 0x00800000 +#define BIT24 0x01000000 +#define BIT25 0x02000000 +#define BIT26 0x04000000 +#define BIT27 0x08000000 +#define BIT28 0x10000000 +#define BIT29 0x20000000 +#define BIT30 0x40000000 +#define BIT31 0x80000000 + +#define STAT_BIT_FLASH_BUSY BIT3 +#define HID_MESSAGE_MAX_SIZE 512 + +typedef struct _hidmessageStruct +{ + struct _hidhead + { + struct _packetcontrolStruct + { + unsigned char dest :3; /* 0 - ProjCtrl; 1 - RFC; 7 - Debugmsg */ + unsigned char reserved :2; + unsigned char nack :1; /* Command Handler Error */ + unsigned char reply :1; /* Host wants a reply from device */ + unsigned char rw :1; /* Write = 0; Read = 1 */ + }flags; + unsigned char seq; + unsigned short length; + }head; + union + { + unsigned short cmd; + unsigned char data[HID_MESSAGE_MAX_SIZE]; + }text; +}hidMessageStruct; + +typedef struct _readCmdData +{ + unsigned char CMD2; + unsigned char CMD3; + unsigned short len; +}CmdFormat; + +typedef struct _rectangle +{ + unsigned short firstPixel; + unsigned short firstLine; + unsigned short pixelsPerLine; + unsigned short linesPerFrame; +}rectangle; + +typedef enum +{ + SOURCE_SEL, + PIXEL_FORMAT, + CLK_SEL, + CHANNEL_SWAP, + FPD_MODE, + CURTAIN_COLOR, + POWER_CONTROL, + FLIP_LONG, + FLIP_SHORT, + TPG_SEL, + PWM_INVERT, + LED_ENABLE, + GET_VERSION, + SW_RESET, + DMD_PARK, + BUFFER_FREEZE, + STATUS_HW, + STATUS_SYS, + STATUS_MAIN, + CSC_DATA, + GAMMA_CTL, + BC_CTL, + PWM_ENABLE, + PWM_SETUP, + PWM_CAPTURE_CONFIG, + GPIO_CONFIG, + LED_CURRENT, + DISP_CONFIG, + TEMP_CONFIG, + TEMP_READ, + MEM_CONTROL, + I2C_CONTROL, + LUT_VALID, + DISP_MODE, + TRIG_OUT1_CTL, + TRIG_OUT2_CTL, + RED_STROBE_DLY, + GRN_STROBE_DLY, + BLU_STROBE_DLY, + PAT_DISP_MODE, + PAT_TRIG_MODE, + PAT_START_STOP, + BUFFER_SWAP, + BUFFER_WR_DISABLE, + CURRENT_RD_BUFFER, + PAT_EXPO_PRD, + INVERT_DATA, + PAT_CONFIG, + MBOX_ADDRESS, + MBOX_CONTROL, + MBOX_DATA, + TRIG_IN1_DELAY, + TRIG_IN2_CONTROL, + SPLASH_LOAD, + SPLASH_LOAD_TIMING, + GPCLK_CONFIG, + PULSE_GPIO_23, + ENABLE_LCR_DEBUG, + TPG_COLOR, + PWM_CAPTURE_READ, + PROG_MODE, + BL_STATUS, + BL_SPL_MODE, + BL_GET_MANID, + BL_GET_DEVID, + BL_GET_CHKSUM, + BL_SET_SECTADDR, + BL_SECT_ERASE, + BL_SET_DNLDSIZE, + BL_DNLD_DATA, + BL_FLASH_TYPE, + BL_CALC_CHKSUM, + BL_PROG_MODE, +}LCR_CMD; + +int LCR_SetInputSource(unsigned int source, unsigned int portWidth); +int LCR_GetInputSource(unsigned int *pSource, unsigned int *portWidth); +int LCR_SetPixelFormat(unsigned int format); +int LCR_GetPixelFormat(unsigned int *pFormat); +int LCR_SetPortClock(unsigned int clock); +int LCR_GetPortClock(unsigned int *pClock); +int LCR_SetDataChannelSwap(unsigned int port, unsigned int swap); +int LCR_GetDataChannelSwap(unsigned int *pPort, unsigned int *pSwap); +int LCR_SetFPD_Mode_Field(unsigned int PixelMappingMode, bool SwapPolarity, unsigned int FieldSignalSelect); +int LCR_GetFPD_Mode_Field(unsigned int *pPixelMappingMode, bool *pSwapPolarity, unsigned int *pFieldSignalSelect); +int LCR_SetPowerMode(bool); +int LCR_SetLongAxisImageFlip(bool); +bool LCR_GetLongAxisImageFlip(); +int LCR_SetShortAxisImageFlip(bool); +bool LCR_GetShortAxisImageFlip(); +int LCR_SetTPGSelect(unsigned int pattern); +int LCR_GetTPGSelect(unsigned int *pPattern); +int LCR_SetPWMInvert(bool invert); +int LCR_GetPWMInvert(bool *inverted); +int LCR_SetLedEnables(bool SeqCtrl, bool Red, bool Green, bool Blue); +int LCR_GetLedEnables(bool *pSeqCtrl, bool *pRed, bool *pGreen, bool *pBlue); +int LCR_GetVersion(unsigned int *pApp_ver, unsigned int *pAPI_ver, unsigned int *pSWConfig_ver, unsigned int *pSeqConfig_ver); +int LCR_SoftwareReset(void); +int LCR_GetStatus(unsigned char *pHWStatus, unsigned char *pSysStatus, unsigned char *pMainStatus); +int LCR_SetPWMEnable(unsigned int channel, bool Enable); +int LCR_GetPWMEnable(unsigned int channel, bool *pEnable); +int LCR_SetPWMConfig(unsigned int channel, unsigned int pulsePeriod, unsigned int dutyCycle); +int LCR_GetPWMConfig(unsigned int channel, unsigned int *pPulsePeriod, unsigned int *pDutyCycle); +int LCR_SetPWMCaptureConfig(unsigned int channel, bool enable, unsigned int sampleRate); +int LCR_GetPWMCaptureConfig(unsigned int channel, bool *pEnabled, unsigned int *pSampleRate); +int LCR_SetGPIOConfig(unsigned int pinNum, bool enAltFunc, bool altFunc1, bool dirOutput, bool outTypeOpenDrain, bool pinState); +int LCR_GetGPIOConfig(unsigned int pinNum, bool *pEnAltFunc, bool *pAltFunc1, bool *pDirOutput, bool *pOutTypeOpenDrain, bool *pState); +int LCR_GetLedCurrents(unsigned char *pRed, unsigned char *pGreen, unsigned char *pBlue); +int LCR_SetLedCurrents(unsigned char RedCurrent, unsigned char GreenCurrent, unsigned char BlueCurrent); +int LCR_SetDisplay(rectangle croppedArea, rectangle displayArea); +int LCR_GetDisplay(rectangle *pCroppedArea, rectangle *pDisplayArea); +int LCR_MemRead(unsigned int addr, unsigned int *readWord); +int LCR_MemWrite(unsigned int addr, unsigned int data); +int LCR_ValidatePatLutData(unsigned int *pStatus); +int LCR_SetPatternDisplayMode(bool external); +int LCR_GetPatternDisplayMode(bool *external); +int LCR_SetTrigOutConfig(unsigned int trigOutNum, bool invert, unsigned int rising, unsigned int falling); +int LCR_GetTrigOutConfig(unsigned int trigOutNum, bool *pInvert,unsigned int *pRising, unsigned int *pFalling); +int LCR_SetRedLEDStrobeDelay(unsigned char rising, unsigned char falling); +int LCR_SetGreenLEDStrobeDelay(unsigned char rising, unsigned char falling); +int LCR_SetBlueLEDStrobeDelay(unsigned char rising, unsigned char falling); +int LCR_GetRedLEDStrobeDelay(unsigned char *, unsigned char *); +int LCR_GetGreenLEDStrobeDelay(unsigned char *, unsigned char *); +int LCR_GetBlueLEDStrobeDelay(unsigned char *, unsigned char *); +int LCR_SetProgrammingMode(bool EnterProgMode); +int LCR_ExitProgrammingMode(void); +int LCR_GetProgrammingMode(bool *ProgMode); +int LCR_GetFlashManID(unsigned short *manID); +int LCR_GetFlashDevID(unsigned long long *devID); +int LCR_GetBLStatus(unsigned char *BL_Status); +int LCR_BLSpecialMode(unsigned int Mode); +int LCR_SetFlashAddr(unsigned int Addr); +int LCR_FlashSectorErase(void); +int LCR_SetDownloadSize(unsigned int dataLen); +int LCR_DownloadData(unsigned char *pByteArray, unsigned int dataLen); +void LCR_WaitForFlashReady(void); +int LCR_SetFlashType(unsigned char Type); +int LCR_CalculateFlashChecksum(void); +int LCR_GetFlashChecksum(unsigned int*checksum); +int LCR_SetMode(bool SLmode); +int LCR_GetMode(bool *pMode); +int LCR_LoadSplash(unsigned int index); +int LCR_GetSplashIndex(unsigned int *pIndex); +int LCR_SetTPGColor(unsigned short redFG, unsigned short greenFG, unsigned short blueFG, unsigned short redBG, unsigned short greenBG, unsigned short blueBG); +int LCR_GetTPGColor(unsigned short *pRedFG, unsigned short *pGreenFG, unsigned short *pBlueFG, unsigned short *pRedBG, unsigned short *pGreenBG, unsigned short *pBlueBG); +int LCR_ClearPatLut(void); +int LCR_AddToPatLut(int TrigType, int PatNum,int BitDepth,int LEDSelect,bool InvertPat, bool InsertBlack,bool BufSwap, bool trigOutPrev); +int LCR_GetPatLutItem(int index, int *pTrigType, int *pPatNum,int *pBitDepth,int *pLEDSelect,bool *pInvertPat, bool *pInsertBlack,bool *pBufSwap, bool *pTrigOutPrev); +int LCR_SendPatLut(void); +int LCR_SendSplashLut(unsigned char *lutEntries, unsigned int numEntries); +int LCR_GetPatLut(int numEntries); +int LCR_GetSplashLut(unsigned char *pLut, int numEntries); +int LCR_SetPatternTriggerMode(bool); +int LCR_GetPatternTriggerMode(bool *); +int LCR_PatternDisplay(int Action); +int LCR_SetPatternConfig(unsigned int numLutEntries, bool repeat, unsigned int numPatsForTrigOut2, unsigned int numSplash); +int LCR_GetPatternConfig(unsigned int *pNumLutEntries, bool *pRepeat, unsigned int *pNumPatsForTrigOut2, unsigned int *pNumSplash); +int LCR_SetExpsosure_FramePeriod(unsigned int exposurePeriod, unsigned int framePeriod); +int LCR_GetExposure_FramePeriod(unsigned int *pExposure, unsigned int *pFramePeriod); +int LCR_SetTrigIn1Delay(unsigned int Delay); +int LCR_GetTrigIn1Delay(unsigned int *pDelay); +int LCR_SetInvertData(bool invert); +int LCR_PWMCaptureRead(unsigned int channel, unsigned int *pLowPeriod, unsigned int *pHighPeriod); +int LCR_SetGeneralPurposeClockOutFreq(unsigned int clkId, bool enable, unsigned int clkDivider); +int LCR_GetGeneralPurposeClockOutFreq(unsigned int clkId, bool *pEnabled, unsigned int *pClkDivider); +int LCR_MeasureSplashLoadTiming(unsigned int startIndex, unsigned int numSplash); +int LCR_ReadSplashLoadTiming(unsigned int *pTimingData); + + +#endif // API_H diff --git a/Classes/CalibrationData.cpp b/Classes/CalibrationData.cpp new file mode 100644 index 0000000..0e2fddb --- /dev/null +++ b/Classes/CalibrationData.cpp @@ -0,0 +1,179 @@ +#include "CalibrationData.h" + +CalibrationData::CalibrationData() : Kc(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), kc(0.0), cam_error(0.0), + Kp(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), kp(0.0), proj_error(0.0), + Rp(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), Tp(0.0), stereo_error(0.0), + frameWidth(1280), frameHeight(1024), screenResX(0), screenResY(0) +{ +} + +CalibrationData::CalibrationData(Matx33d _Kc, Vec _kc, double _cam_error, Matx33d _Kp, Vec _kp, + double _proj_error, Matx33d _Rp, Vec3f _Tp, double _stereo_error) : + Kc(_Kc), kc(_kc), cam_error(_cam_error), Kp(_Kp), kp(_kp), proj_error(_proj_error), Rp(_Rp), Tp(_Tp), + stereo_error(_stereo_error) +{ + auto cArg = CameraArguments::getInstance(cvtools::Matx33dToMat(Rp), cvtools::Vec3fToMat(Tp), cvtools::Matx33dToMat(_Kc), + cvtools::Matx33dToMat(_Kp)); +} + +bool CalibrationData::load(const QString& filename) +{ + QFileInfo info(filename); + // QString type = info.suffix(); + + if (info.exists() && info.suffix() == "xml") + return loadXML(filename); + else + { + std::cerr << "CalibrationData error: no such .xml file: " << filename.toStdString() << std::endl; + return false; + } +} + +bool CalibrationData::save(const QString& filename) +{ + QFileInfo info(filename); + QString type = info.suffix(); + + if (type == "xml") + { + return saveXML(filename); + } + else if (type == "slcalib") + { + return saveSLCALIB(filename); + } + else if (type == "m") + { + return saveMatlab(filename); + } + else + { + std::cerr << "CalibrationData error save: unknown file extension: " << type.toStdString() << std::endl; + return false; + } + + return false; +} + +bool CalibrationData::loadXML(const QString& filename) +{ + FileStorage fs(filename.toStdString(), FileStorage::READ); // + if (!fs.isOpened()) + { + std::cerr << "CalibrationData error: could not open file " << filename.toStdString() << std::endl; + return false; + } + + Mat temp; + fs["Kc"] >> temp; + Kc = temp; + fs["kc"] >> temp; + kc = temp; + fs["Kp"] >> temp; + Kp = temp; + fs["kp"] >> temp; + kp = temp; + fs["Rp"] >> temp; + Rp = temp; + fs["Tp"] >> temp; + Tp = temp; + + fs["cam_error"] >> cam_error; + fs["proj_error"] >> proj_error; + fs["stereo_error"] >> stereo_error; + + fs["frameWidth"] >> frameWidth; + fs["frameHeight"] >> frameHeight; + fs["screenResX"] >> screenResX; + fs["screenResY"] >> screenResY; + + fs.release(); + + return true; +} + +bool CalibrationData::saveSLCALIB(const QString& filename) +{ + FILE* fp = fopen(qPrintable(filename), "w"); + if (!fp) + return false; + + fprintf(fp, "#V1.0 SLStudio calibration\n"); + fprintf(fp, "#Calibration time: %s\n\n", calibrationDateTime.c_str()); + fprintf(fp, "Kc\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kc(0, 0), Kc(0, 1), Kc(0, 2), Kc(1, 0), Kc(1, 1), Kc(1, 2), + Kc(2, 0), Kc(2, 1), Kc(2, 2)); + fprintf(fp, "kc\n%f %f %f %f %f\n\n", kc(0), kc(1), kc(2), kc(3), kc(4)); + fprintf(fp, "Kp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kp(0, 0), Kp(0, 1), Kp(0, 2), Kp(1, 0), Kp(1, 1), Kp(1, 2), + Kp(2, 0), Kp(2, 1), Kp(2, 2)); + fprintf(fp, "kp\n%f %f %f %f %f\n\n", kp(0), kp(1), kp(2), kp(3), kp(4)); + fprintf(fp, "Rp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Rp(0, 0), Rp(0, 1), Rp(0, 2), Rp(1, 0), Rp(1, 1), Rp(1, 2), + Rp(2, 0), Rp(2, 1), Rp(2, 2)); + fprintf(fp, "Tp\n%f %f %f\n\n", Tp(0), Tp(1), Tp(2)); + + fprintf(fp, "cam_error: %f\n\n", cam_error); + fprintf(fp, "proj_error: %f\n\n", proj_error); + fprintf(fp, "stereo_error: %f\n\n", stereo_error); + + fclose(fp); + + return true; +} + +bool CalibrationData::saveXML(const QString& filename) +{ + FileStorage fs(filename.toStdString(), FileStorage::WRITE); + if (!fs.isOpened()) + return false; + + fs << "Kc" << Mat(Kc) << "kc" << Mat(kc) + << "Kp" << Mat(Kp) << "kp" << Mat(kp) + << "Rp" << Mat(Rp) << "Tp" << Mat(Tp) + << "cam_error" << cam_error + << "proj_error" << proj_error + << "stereo_error" << stereo_error + << "frameWidth" << frameWidth + << "frameHeight" << frameHeight + << "screenResX" << screenResX + << "screenResY" << screenResY; + fs.release(); + + return true; +} + +bool CalibrationData::saveMatlab(const QString& filename) +{ + std::ofstream file(qPrintable(filename)); + if (!file) + return false; + + file << "%%SLStudio calibration" << std::endl; + file << "Kc = " << Kc << ";" << std::endl; + file << "kc = " << kc << ";" << std::endl; + file << "Kp = " << Kp << ";" << std::endl; + file << "kp = " << kp << ";" << std::endl; + file << "Rp = " << Rp << ";" << std::endl; + file << "Tp = " << Tp << ";" << std::endl; + + file.close(); + + return true; +} + +void CalibrationData::print() +{ + std::cout << std::setw(5) << std::setprecision(4) + << "========================================\n" + << "Camera Calibration: \n" + << "- cam_error:\n" << cam_error << "\n" + << "- Kc:\n" << Kc << "\n" + << "- kc:\n" << kc << "\n" + << "Projector Calibration: " << "\n" + << "- proj_error: \n" << proj_error << "\n" + << "- Kp: \n" << Kp << "\n" + << "- kp: \n" << kp << "\n" + << "Stereo Calibration: \n" + << "- stereo_error:\n" << stereo_error << "\n" + << "- Rp:\n" << Rp << "\n" + << "- Tp:\n" << Tp << std::endl; +} diff --git a/Classes/CalibrationData.h b/Classes/CalibrationData.h new file mode 100644 index 0000000..3803dea --- /dev/null +++ b/Classes/CalibrationData.h @@ -0,0 +1,49 @@ +#ifndef CALIBRATIONDATA_H +#define CALIBRATIONDATA_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "cvtools.h" +#include "CameraArguments.h" + +using namespace cv; + +class CalibrationData{ + public: + CalibrationData(); + CalibrationData(cv::Matx33d _Kc, cv::Vec _kc, double _cam_error, cv::Matx33d _Kp, cv::Vec _kp, + double _proj_error, cv::Matx33d _Rp, cv::Vec3f _Tp, double _stereo_error); + bool load(const QString& filename); + bool save(const QString& filename); + bool loadXML(const QString& filename); + bool saveXML(const QString& filename); + bool saveMatlab(const QString& filename); + bool saveSLCALIB(const QString& filename); + void print(); + + Matx33d Kc; // Intrinsic camera matrix + Vec kc; // Camera distortion coefficients + double cam_error; + + Matx33d Kp; // Intrinsic projector matrix + Vec kp; // Projector distortion coefficients + double proj_error; + + Matx33d Rp; // Extrinsic camera rotation matrix + Vec3f Tp; // Extrinsic camera rotation matrix + + double stereo_error; + + int frameWidth , frameHeight; + int screenResX , screenResY; + std::string calibrationDateTime; +}; + +#endif diff --git a/Classes/Calibrator.cpp b/Classes/Calibrator.cpp new file mode 100644 index 0000000..74dd772 --- /dev/null +++ b/Classes/Calibrator.cpp @@ -0,0 +1,338 @@ +#include "Calibrator.h" +#include "structured_light.h" +#include "CalibrationData.h" + +#include + +#include + +Calibrator::Calibrator() +{ + +} + +Calibrator::~Calibrator() +{ + +} + +void Calibrator::addFrameSequence(std::vector &frameSeq) +{ + int n = frameSeqs.size(); + frameSeqs.resize(n+1); + std::vector &frame = frameSeqs[n]; + for(int i = 0; i < frameSeq.size(); i++) + { + frame.push_back(frameSeq[i]); + } + std::cout <<"Calibrator::addFrameSequence : frame.size(): "<< frame.size() << std::endl; +} + +void Calibrator::reset() +{ + frameSeqs.clear(); + board_corners.clear(); + projector_corners.clear(); + pattern_list.clear(); +} + +void Calibrator::setBoardRows(int rows) { + board_rows = rows; + board_size = cv::Size(board_rows, board_cols); +} + +void Calibrator::setBoardCols(int cols) { + board_cols = cols; + board_size = cv::Size(board_rows, board_cols); +} + +void Calibrator::setCornerSize(double cornerSize) { + dot_dis = cornerSize; + corner_size = cv::Size2f(cornerSize, cornerSize); +} + +void Calibrator::setDotDiameter(double dotDiameter) { + dot_diameter = dotDiameter; +} +void Calibrator::setCalibBoard(unsigned board) { + board_type = board; +} + +CalibrationData* Calibrator::calibrate() +{ + + CalibrationData *res = new CalibrationData(); + +// detect corners //////////////////////////////////// + for(int i = 0; i < frameSeqs.size(); i++) + { + std::vector corners = extract_board_corners( frameSeqs[i][0] ); + board_corners.push_back(corners); +// std::cout< pcorners; + for(int i = 0; i < frameSeqs.size(); i++) + { + std::vector const& corners = board_corners[i]; + cv::Mat pattern_image; + cv::Mat min_max_image; + if(corners.size()==0) + { + projector_corners.push_back(pcorners); + pattern_list.push_back(pattern_image); + continue; + } + if(!decode_gray_set(i , pattern_image ,min_max_image)) + { + projector_corners.push_back(pcorners); + pattern_list.push_back(pattern_image); + continue; + } + + pattern_list.push_back(pattern_image); + for (std::vector::const_iterator iter=corners.begin(); iter!=corners.end(); iter++) + { + const cv::Point2f & p = *iter; + cv::Point2f q; + //find an homography around p + unsigned WINDOW_SIZE = 30; + std::vector img_points , proj_points; + if (p.x>WINDOW_SIZE && p.y>WINDOW_SIZE && p.x+WINDOW_SIZE(h); + register const cv::Vec2b * min_max_row = min_max_image.ptr(h); + //cv::Vec2f * out_row = out_pattern_image.ptr(h); + for (unsigned w=p.x-WINDOW_SIZE; w0 || std::isnan(pattern[1])>0) + { + continue; + } + if ((min_max[1]-min_max[0]) < static_cast(threshold)) + { //apply threshold and skip + continue; + } + + img_points.push_back(cv::Point2f(w, h)); + proj_points.push_back(cv::Point2f(pattern)); + + //out_pattern = pattern; + } + } + cv::Mat H = cv::findHomography(img_points , proj_points, cv::RANSAC); +// std::cout << " H:\n" << H << std::endl; + cv::Point3d Q = cv::Point3d(cv::Mat(H*cv::Mat(cv::Point3d(p.x , p.y , 1.0)))); + q = cv::Point2f(Q.x/Q.z ,Q.y/Q.z); + // jiuzheng + // q.y += 118; + pcorners.push_back(q); + } + else { + pcorners.clear(); + break; + } + } + std::cout< world_corners; + for (int h=0; h > objectPoints; + objectPoints.reserve(count); + for (unsigned i=0; i world_corners_p; + for (int h=0; h > objectPoints_p; + objectPoints_p.reserve(count); + for (unsigned i=0; i cam_rvecs , cam_tvecs; + int cam_flags = cal_flags; + cv::Size imageSize = frameSeqs[0][0].size(); + res->cam_error = cv::calibrateCamera(objectPoints , board_corners , imageSize , res->Kc , res->kc ,cam_rvecs , cam_tvecs); + std::cout<<"calibrate the camera !"< proj_rvecs , proj_tvecs; + int proj_flags = cal_flags; + cv::Size projector_size(912, 1140); + res->proj_error = cv::calibrateCamera(objectPoints_p , projector_corners, projector_size , res->Kp , res->kp , proj_rvecs , proj_tvecs); + + std::cout<<"calibrate the projector !"<stereo_error = cv::stereoCalibrate(objectPoints , board_corners , projector_corners , + res->Kc , res->kc , res->Kp ,res->kp ,imageSize /*ignored*/ , + res->Rp , res->Tp , E , F ); +// res->stereo_error = cv::stereoCalibrate(objectPoints , board_corners , projector_corners , res->Kc , res->kc , res->Kp ,res->kp ,imageSize /*ignored*/ , res->Rp , res->Tp , E , F , +// cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS ,50 ,DBL_EPSILON) , +// cv::CALIB_FIX_INTRINSIC /*cv::CALIB_USE_INTRINSIC_GUESS + cal_flags*/); + std::cout<<"stereo calibration !"< Calibrator::extract_board_corners(cv::Mat &gray_image) +{ + std::vector corners; + int image_scale = 1; + if (gray_image.rows<1) + { + return corners; + } + cv::Size imageSize = gray_image.size(); + if (imageSize.width>1024) + { + image_scale = imageSize.width/1024; + } + + cv::Mat small_img; + if (image_scale>1) + { + cv::resize(gray_image , small_img , cv::Size(gray_image.cols/image_scale , gray_image.rows/image_scale)); + } + else + { + small_img = gray_image; + } + + if(board_type == Chessboard) { + cv::findChessboardCorners(small_img, board_size, corners); + } + else if(board_type == Circular) { + cv::findCirclesGrid(small_img, board_size, corners); + } + + if (corners.size()) + { + for (std::vector::iterator iter=corners.begin(); iter!=corners.end(); iter++) + { + *iter = image_scale*(*iter); + } + cv::cornerSubPix(gray_image , corners , cv::Size(11 , 11) , cv::Size(-1 , -1) , + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30 , 0.1)); + } + + return corners; +} + + +bool Calibrator::decode_gray_set(int ind , cv::Mat & pattern_image, cv::Mat & min_max_image) +{ + if (ind >= frameSeqs.size()) + { //out of bounds + return false; + } + + //estimate direct component + //b = config.value("robust_estimation/b" DEFAULT_B).toFloat(); + std::vector images; +// QList direct_component_images(QList() << 15 << 16 << 17 << 18 << 35 << 36 << 37 << 38); + int total_images = frameSeqs[ind].size(); + int total_patterns = total_images/2 - 1; + const int direct_light_count = 4; + const int direct_light_offset = 4; + + QList direct_component_images; + for (unsigned i=0; i +#include + +#include "CalibrationData.h" + +class Calibrator +{ +public: + Calibrator(); + ~Calibrator(); + void addFrameSequence(std::vector &frameSeq); + void reset(); + CalibrationData * calibrate(); + std::vector extract_board_corners(cv::Mat &gray_image); + bool decode_gray_set(int i , cv::Mat & pattern_image, cv::Mat & min_max_image); + void setBoardRows(int rows); + void setBoardCols(int cols); + void setCornerSize(double cornerSize); + void setDotDiameter(double dotDiameter); + void setCalibBoard(unsigned board); + + const static unsigned Chessboard = 0x00; + const static unsigned Circular = 0x01; + +protected: + std::vector< std::vector > frameSeqs; + std::vector< std::vector > board_corners; + std::vector< std::vector > projector_corners; + std::vector pattern_list; + + cv::Size2f corner_size = cv::Size2f(20.f , 20.f); + cv::Size board_size = cv::Size(8 , 6); + int board_rows = 8; + int board_cols = 6; + double dot_dis = 20.0; + + double dot_diameter = 7.5; + + unsigned board_type = Chessboard; + + unsigned threshold = 10; + float b = 0.5; + unsigned m = 5; + +}; + +#endif // CALIBRATOR_H diff --git a/Classes/Camera.cpp b/Classes/Camera.cpp new file mode 100644 index 0000000..2c4453a --- /dev/null +++ b/Classes/Camera.cpp @@ -0,0 +1,28 @@ +#include "Camera.h" +#include + + +#include "CameraPointGrey.h" + +// Global camera enumerator +std::vector< std::vector > Camera::GetInterfaceCameraList(){ + std::vector< std::vector > ret; + +#ifdef WITH_CAMERAPOINTGREY + std::vector ptgreycameras = CameraPointGrey::getCameraList(); + ret.push_back(ptgreycameras); +#endif + + return ret; +} + +// Camera factory +Camera* Camera::NewCamera(unsigned int interfaceNum , unsigned int camNum , CameraTriggerMode triggerMode){ + + std::cout << "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa" << std::endl; + + if(interfaceNum == 0) + return new CameraPointGrey(camNum , triggerMode); + + return (Camera*)NULL; +} diff --git a/Classes/Camera.h b/Classes/Camera.h new file mode 100644 index 0000000..2cf8a11 --- /dev/null +++ b/Classes/Camera.h @@ -0,0 +1,60 @@ +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include + +struct CameraFrame { + unsigned char *memory; + unsigned int width; + unsigned int height; + unsigned int sizeBytes; + unsigned int timeStamp; + CameraFrame(): memory(NULL), width(0), height(0), sizeBytes(0), timeStamp(0){} +}; + +struct CameraSettings { + float gain; + float shutter; // [ms] + CameraSettings(): gain(0.0), shutter(0.0) {} +}; + +struct CameraInfo { + std::string vendor; + std::string model; + unsigned int busID; +}; + +enum CameraTriggerMode { + triggerModeHardware , + triggerModeSoftware +}; + +// Camera factory methods and abstract base class for camera implementations +class Camera { + + public: + // Static "camera factory" methods + static std::vector< std::vector > GetInterfaceCameraList(); + static Camera* NewCamera(unsigned int interfaceNum, unsigned int camNum, CameraTriggerMode triggerMode); + // Interface function + Camera(CameraTriggerMode _triggerMode) : capturing(false), connecting(false), triggerMode(_triggerMode){} + virtual void startCapture() = 0; + bool isCapturing(){return capturing;} + bool isConnecting(){return connecting;} + virtual void stopCapture() = 0; + virtual CameraFrame getFrame() = 0; + virtual size_t getFrameSizeBytes() = 0; + virtual size_t getFrameWidth() = 0; + virtual size_t getFrameHeight() = 0; + virtual CameraSettings getCameraSettings() = 0; + virtual void setCameraSettings(CameraSettings) = 0; + virtual ~Camera(){ } + + protected: + bool capturing; + bool connecting; + CameraTriggerMode triggerMode; +}; + +#endif diff --git a/Classes/CameraArguments.cpp b/Classes/CameraArguments.cpp index b0d13bd..1994e67 100644 --- a/Classes/CameraArguments.cpp +++ b/Classes/CameraArguments.cpp @@ -17,8 +17,8 @@ CameraArguments::CameraArguments(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp) kc1 = kc; kp2 = kp; cv::Mat tmp; - hconcat(cv::Mat::eye(3, 3,CV_32FC1), - cv::Mat::zeros(cv::Size(5, 5), CV_32FC1), tmp); + hconcat(cv::Mat::eye(3, 3, CV_32FC1), + cv::Mat::zeros(cv::Size(1, 3), CV_32FC1), tmp); hc1 = kc1 * tmp; hconcat(r12, t12.t(), tmp); hp2 = kp2 * tmp; diff --git a/Classes/CameraPointGrey.cpp b/Classes/CameraPointGrey.cpp new file mode 100644 index 0000000..3ecdd1f --- /dev/null +++ b/Classes/CameraPointGrey.cpp @@ -0,0 +1,306 @@ +#include "CameraPointGrey.h" +#include + +void PrintError(FlyCapture2::Error error){ + error.PrintErrorTrace(); +} + +vector CameraPointGrey::getCameraList(){ + + FlyCapture2::Error error; + + FlyCapture2::BusManager busManager; + unsigned int numCameras; + error = busManager.GetNumOfCameras(&numCameras); + + vector ret; + + if (error != FlyCapture2::PGRERROR_OK){ + PrintError(error); + return ret; + } + + for (unsigned int i=0; i < numCameras; i++){ + FlyCapture2::PGRGuid guid; + error = busManager.GetCameraFromIndex(i , &guid); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + // Connect to camera + FlyCapture2::Camera cam; + error = cam.Connect(&guid); + if (error != FlyCapture2::PGRERROR_OK) + PrintError( error ); + + // Get the camera information + FlyCapture2::CameraInfo camInfo; + error = cam.GetCameraInfo(&camInfo); + if (error != FlyCapture2::PGRERROR_OK) + PrintError( error ); + + CameraInfo camera; + camera.busID = camInfo.nodeNumber; + camera.model = camInfo.modelName; + camera.vendor = "Point Grey Research"; + + ret.push_back(camera); + } + + return ret; +} + +CameraPointGrey::CameraPointGrey(unsigned int camNum , CameraTriggerMode triggerMode) : Camera(triggerMode){ + + FlyCapture2::Error error; + + // Connect to camera + FlyCapture2::BusManager busManager; + FlyCapture2::PGRGuid camGuid; + busManager.GetCameraFromIndex(camNum , &camGuid); + error = cam.Connect(&camGuid); + if (error != FlyCapture2::PGRERROR_OK) { + PrintError(error); + connecting = false; + return ; + } + + FlyCapture2::Format7ImageSettings format7Settings; + format7Settings.mode = FlyCapture2::MODE_0; + format7Settings.pixelFormat = FlyCapture2::PIXEL_FORMAT_RGB8; + format7Settings.width = 1280; + format7Settings.height = 1024; + format7Settings.offsetX = 0; + format7Settings.offsetY = 0; + + // Validate and set mode + FlyCapture2::Format7PacketInfo packetInfo; + bool valid; + error = cam.ValidateFormat7Settings(&format7Settings , &valid ,&packetInfo); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + // packetsize configures maximum frame rate + error = cam.SetFormat7Configuration(&format7Settings, packetInfo.recommendedBytesPerPacket); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + // Turn off gamma + FlyCapture2::Property property; + property.type = FlyCapture2::AUTO_EXPOSURE; + property.onOff = false; + error = cam.SetProperty(&property); + if (error != FlyCapture2::PGRERROR_OK){ + PrintError(error); + connecting = false; + return ; + } + + property.type = FlyCapture2::GAMMA; + property.onOff = true; + property.absControl = true; + property.absValue = 1.0; + error = cam.SetProperty(&property); + if (error != FlyCapture2::PGRERROR_OK){ + PrintError(error); + connecting = false; + return ; + } + + + // Get the camera information + FlyCapture2::CameraInfo camInfo; + error = cam.GetCameraInfo(&camInfo); + if (error != FlyCapture2::PGRERROR_OK){ + PrintError(error); + connecting = false; + return ; + } + + std::cout << camInfo.vendorName << " " << camInfo.modelName << " " << camInfo.serialNumber << std::endl; + connecting = true; + + // Set reasonable default settings + CameraSettings settings; + //settings.shutter = 8.33; + settings.shutter = 33.33; + settings.gain = 0.0; + this->setCameraSettings(settings); + + + return; +} + +CameraSettings CameraPointGrey::getCameraSettings(){ + + FlyCapture2::Property property; + + // Get settings: + CameraSettings settings; + + property.type = FlyCapture2::SHUTTER; + cam.GetProperty(&property); + settings.shutter = property.absValue; + + property.type = FlyCapture2::GAIN; + cam.GetProperty(&property); + settings.gain = property.absValue; + + return settings; +} + +void CameraPointGrey::setCameraSettings(CameraSettings settings){ + /* + FlyCapture2::Property property; + property.onOff = true; + property.absControl = true; + + property.type = FlyCapture2::SHUTTER; + property.absValue = settings.shutter; + cam.SetProperty(&property); + + property.type = FlyCapture2::GAIN; + property.absValue = settings.gain; + cam.SetProperty(&property); + */ + +} + +void CameraPointGrey::startCapture(){ + + FlyCapture2::Error error; + + CameraSettings settings = this->getCameraSettings(); + std::cout << "\tShutter: " << settings.shutter << "ms" << std::endl; + std::cout << "\tGain: " << settings.gain << "dB" << std::endl; + + if(triggerMode == triggerModeHardware){ + // Configure for hardware trigger + FlyCapture2::TriggerMode triggerMode; + triggerMode.onOff = true; + triggerMode.polarity = 0; + triggerMode.source = 0; + triggerMode.mode = 14; + error = cam.SetTriggerMode(&triggerMode); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + error = cam.StartCapture(); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + } else if(triggerMode == triggerModeSoftware){ + // Configure software trigger + FlyCapture2::TriggerMode triggerMode; + triggerMode.onOff = true; + triggerMode.polarity = 0; + triggerMode.source = 7; // software + triggerMode.mode = 0; + error = cam.SetTriggerMode(&triggerMode); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + } + + // Set the trigger timeout to 1000 ms + FlyCapture2::FC2Config config; + config.grabTimeout = 1000; + error = cam.SetConfiguration(&config); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + error = cam.StartCapture(); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + capturing = true; +} + +void CameraPointGrey::stopCapture(){ + + FlyCapture2::Error error = cam.StopCapture(); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + capturing = false; +} + +CameraFrame CameraPointGrey::getFrame(){ + + FlyCapture2::Error error; + + if(triggerMode == triggerModeSoftware){ + // Fire software trigger + // broadcasting not supported on some platforms + cam.FireSoftwareTrigger(false); + } + + // Retrieve the image + FlyCapture2::Image rawImage; + error = cam.RetrieveBuffer(&rawImage); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + CameraFrame frame; + + frame.timeStamp = rawImage.GetTimeStamp().cycleCount; + frame.height = rawImage.GetRows(); + frame.width = rawImage.GetCols(); + frame.memory = rawImage.GetData(); + + return frame; +} + + +size_t CameraPointGrey::getFrameSizeBytes(){ + + FlyCapture2::Format7ImageSettings format7Settings; + unsigned int dummy1; + float dummy2; + FlyCapture2::Error error = cam.GetFormat7Configuration(&format7Settings , &dummy1, &dummy2); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + return format7Settings.width*format7Settings.height; +} + +size_t CameraPointGrey::getFrameWidth(){ + + FlyCapture2::Format7ImageSettings format7Settings; + unsigned int dummy1; + float dummy2; + FlyCapture2::Error error = cam.GetFormat7Configuration(&format7Settings, &dummy1 , &dummy2); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + return format7Settings.width; + +} + +size_t CameraPointGrey::getFrameHeight(){ + + FlyCapture2::Format7ImageSettings format7Settings; + unsigned int dummy1; + float dummy2; + FlyCapture2::Error error = cam.GetFormat7Configuration(&format7Settings , &dummy1 , &dummy2); + if (error != FlyCapture2::PGRERROR_OK) + PrintError(error); + + return format7Settings.height; + +} + + +CameraPointGrey::~CameraPointGrey(){ + + if(capturing){ + // Stop camera transmission + cam.StopCapture(); + } + + // Gracefulle destruct the camera + cam.Disconnect(); + + std::cout<<"camera disconnect"< + +using namespace std; + +class CameraPointGrey : public Camera { + public: + // Static methods + static vector getCameraList(); + // Interface function + CameraPointGrey(unsigned int camNum , CameraTriggerMode triggerMode); + CameraSettings getCameraSettings(); + void setCameraSettings(CameraSettings); + void startCapture(); + void stopCapture(); + CameraFrame getFrame(); + size_t getFrameSizeBytes(); + size_t getFrameWidth(); + size_t getFrameHeight(); + ~CameraPointGrey(); + private: + FlyCapture2::Camera cam; +}; + +#endif diff --git a/Classes/Common.h b/Classes/Common.h new file mode 100644 index 0000000..094241e --- /dev/null +++ b/Classes/Common.h @@ -0,0 +1,108 @@ +/* + * Common.h + * + * This module provides the common defines used by all the modules. + * + * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/ + * ALL RIGHTS RESERVED + * +*/ + +#ifndef COMMON_H_ +#define COMMON_H_ + +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + +#define TRUE 1 +#define FALSE 0 + +typedef enum +{ + IMAGE_PIX_FORMAT_RGB32, + IMAGE_PIX_FORMAT_GREY8, + IMAGE_PIX_FORMAT_GREY10, + IMAGE_PIX_FORMAT_UYVY16, + IMAGE_PIX_FORMAT_RGB16, + IMAGE_PIX_FORMAT_SBGGR, + IMAGE_PIX_FORMAT_RGB24, +} ImagePixFormat_t; + +typedef struct +{ + unsigned char *Buffer; + unsigned Width; + unsigned Height; + unsigned LineWidth; + ImagePixFormat_t PixFormat; +}Image_t; + + +typedef int BOOL; +typedef unsigned uint32; +typedef unsigned char uint8; +typedef unsigned short uint16; + +typedef uint16 PatternCount_t; + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define ALIGN_BYTES_PREV(x, b) ((x) & ~(uint32)((b) - 1)) +#define ALIGN_BYTES_NEXT(x, b) (((x) + ((b)-1)) & ~(uint32)((b) - 1)) + +#define GET_BYTE0(x) ((x) & 0xFF) +#define GET_BYTE1(x) (((x) >> 8) & 0xFF) +#define GET_BYTE2(x) (((x) >> 16) & 0xFF) +#define GET_BYTE3(x) (((x) >> 24) & 0xFF) + +#define PARSE_WORD16_LE(Arr) MAKE_WORD16((Arr)[1], (Arr)[0]) +#define PARSE_WORD16_BE(Arr) MAKE_WORD16((Arr)[0], (Arr)[1]) +#define PARSE_WORD24_LE(Arr) MAKE_WORD32(0, (Arr)[2], (Arr)[1], (Arr)[0]) +#define PARSE_WORD24_BE(Arr) MAKE_WORD32(0, (Arr)[0], (Arr)[1], (Arr)[2]) +#define PARSE_WORD32_LE(Arr) MAKE_WORD32((Arr)[3], (Arr)[2], (Arr)[1], (Arr)[0]) +#define PARSE_WORD32_BE(Arr) MAKE_WORD32((Arr)[0], (Arr)[1], (Arr)[2], (Arr)[3]) + +#define MAKE_WORD16(b1, b0) (((b1) << 8) | (b0)) +#define MAKE_WORD32(b3, b2, b1, b0) (((uint32)(b3)<<24)|((uint32)(b2)<<16)|((uint32)(b1)<<8)|(b0)) + +#define ARRAY_SIZE(x) (sizeof(x)/sizeof(*x)) +#define DIV_ROUND(x, y) (((x)+(y)/2)/(y)) +#define DIV_CEIL(x, y) (((x)+(y)-1)/(y)) +#define POW_OF_2(x) (1ul << (x)) + +#define IS_POW_OF_2(x) (((x) & ((x)-1)) == 0) + +// Generate bit mask of n bits starting from s bit +#define GEN_BIT_MASK(s, n) (((1ul << (n)) - 1) << (s)) + +// Merge bits b into a according to mask +#define MERGE_BITS(a, b, mask) ((a) ^ (((a) ^ (b)) & (mask))) + +#define STRUCT_CLEAR(s) (memset(&(s), 0, sizeof(s))) +#define BIT_REVERSE(x) CMN_BitRevLUT[(uint8)(x)] + +#define STR(x) STR_(x) +#define STR_(x) #x + +extern const uint8 CMN_BitRevLUT[]; +void PrintArray(char const *Prefix, uint8 const *Array, unsigned Size, BOOL Hex); +uint32 GetImagePixel(Image_t *Image, int x, int y); +uint32 Next2Power(uint32 Value); +unsigned Hex2BinArray(char *HexStr, unsigned Size, uint8 *BinArray); +int TrimString(char const *Input, char *Output); +int WriteTextToFile(char const *File, int Num, char const *Text); +int ReadTextFromFile(char const *File, int Num, char *Text, int Len); +BOOL FileExist(char const *File, int Num); + +#ifdef __cplusplus +} +#endif + + +#endif /* COMMON_H_ */ diff --git a/Classes/CoreAlgorithm.cpp b/Classes/CoreAlgorithm.cpp index 6778139..1f3ebfa 100644 --- a/Classes/CoreAlgorithm.cpp +++ b/Classes/CoreAlgorithm.cpp @@ -1,76 +1,111 @@ #include "CoreAlgorithm.h" -#include -CoreAlgorithm::CoreAlgorithm(std::string path = "../Data/image/reconstruction/tq.png") +CoreAlgorithm::CoreAlgorithm(const std::string& path, CameraArguments* cArgs) { - image = cv::imread(path, cv::IMREAD_COLOR); - tmp = image.clone(); // 深拷贝 - split(image, channel); //b,g,r - cvtColor(image, hsv, COLOR_BGR2HSV); - cArg = CameraArguments::getInstance(); + image = imread(path, IMREAD_COLOR); + rows = image.rows; + cols = image.cols; + split(image, rgbChannel); //b,g,r + hsv = image.clone(); + cvtColor(image, hsv, COLOR_BGR2HSV, 3); + split(hsv, hsvChannel); + cvtColor(image, lab, COLOR_BGR2Lab); + cArg = cArgs; } CoreAlgorithm::~CoreAlgorithm() = default; -Mat CoreAlgorithm::OSTU(Mat src) +Mat CoreAlgorithm::OtsuAlgThreshold(Mat& src) { - const auto width = src.cols; - const auto height = src.rows; - int hisData[256] = {0}; - - for (auto j = 0; j < height; j++) + if (src.channels() != 1) { - auto* data = src.ptr(j); - for (auto i = 0; i < width; i++) - hisData[data[i]]++; + cout << "Please input Gray-src!" << endl; } - auto t0 = 0; + auto T = 0; + double varValue = 0; + double w0 = 0; + double w1 = 0; + double u0 = 0; + double u1 = 0; + double Histogram[256] = {0}; + uchar* data = src.data; - for (auto i = 0; i < 256; i++) + double totalNum = src.rows * src.cols; + + for (auto i = 0; i < src.rows; i++) { - t0 += i * hisData[i]; - } - - t0 /= width * height; - - auto t1 = 0, t2 = 0; - auto num1 = 0, num2 = 0; - auto t = 0; - - while (true) - { - for (auto i = 0; i < t0 + 1; i++) + for (auto j = 0; j < src.cols; j++) { - t1 += i * hisData[i]; - num1 += hisData[i]; + if (src.at(i, j) != 0) Histogram[data[i * src.step + j]]++; } - - if (num1 == 0) continue; - - for (auto i = t0 + 1; i < 256; i++) - { - t2 += i * hisData[i]; - num2 += hisData[i]; - } - - if (num2 == 0) continue; - - t = (t1 / num1 + t2 / num2) / 2; - - if (t != t0) t0 = t; - else break; } - Mat dst; + auto minpos = 0, maxpos = 0; + for (auto i = 0; i < 255; i++) + { + if (Histogram[i] != 0) + { + minpos = i; + break; + } + } - threshold(src, dst, t, 255, 0); + for (auto i = 255; i > 0; i--) + { + if (Histogram[i] != 0) + { + maxpos = i; + break; + } + } + for (auto i = minpos; i <= maxpos; i++) + { + w1 = 0; + u1 = 0; + w0 = 0; + u0 = 0; + for (auto j = 0; j <= i; j++) + { + w1 += Histogram[j]; + u1 += j * Histogram[j]; + } + if (w1 == 0) + { + break; + } + u1 = u1 / w1; + w1 = w1 / totalNum; + for (auto k = i + 1; k < 255; k++) + { + w0 += Histogram[k]; + u0 += k * Histogram[k]; + } + if (w0 == 0) + { + break; + } + u0 = u0 / w0; + w0 = w0 / totalNum; + + auto varValueI = w0 * w1 * (u1 - u0) * (u1 - u0); + if (varValue < varValueI) + { + varValue = varValueI; + T = i; + } + } + // cout << T << endl; + Mat dst = src.clone(); + for (auto i = 0; i < src.rows; i++) + for (auto j = 0; j < src.cols; j++) + dst.at(i, j) = src.at(i, j) > T ? 255 : 0; return dst; } -vector CoreAlgorithm::Debruijn(int k, int n) +vector CoreAlgorithm::DeBruijn(int k, int n) { std::vector a(k * n, 0); std::vector seq; @@ -118,107 +153,149 @@ vector CoreAlgorithm::Debruijn(int k, int n) return res; } -vector CoreAlgorithm::Hsv(int r, int g, int b) +void CoreAlgorithm::Reconstruction(vector> maximas, vector> minimas, + vector> colorLabel, vector> phases, const Mat& Hc1, + Mat Hp2, const double* map) { - vector h = {0, 0, 0, 0}; - h[0] = (2 * r - g - b) / (2 * sqrt(pow((r - g), 2) + (r - b) * (g - b))); - h[1] = (2 * g - r - b) / (2 * sqrt(pow((g - r), 2) + (g - b) * (r - b))); - h[2] = (2 * b - g - r) / (2 * sqrt(pow((b - g), 2) + (b - r) * (g - r))); - h[3] = sqrt(1 - ((r * g + g * b + r * b) / (pow(r, 2) + pow(g, 2) + pow(b, 2)))); // 色彩强度 - return h; -} - -Mat CoreAlgorithm::Reconstruction(Mat featurePoint, int num, Mat Hc1, Mat Hp2, Mat map) -{ - auto numOfCoord = 0; - Mat coordinate = Mat::zeros(cv::Size(num, 3), CV_32FC1); - for (auto column = minX; column < maxX; column++) + for (auto i = 0; i < maximas.size(); i++) { - Mat rowPosition = Mat::zeros(cv::Size(700, 1), CV_32FC1); - auto counter = 0; - for (auto row = minY; row < maxY; row++) + if (maximas[i].empty())continue; + if (maximas[i].size() < 4)continue; + auto mark = 0; + // double pc = 0; + for (auto j = 0; j < maximas[i].size(); j++) { - if (featurePoint.at(row, column) >= 0) + double position; + if (j < maximas[i].size() - 3) { - rowPosition.at(0, counter) = row; - counter++; + position = map[int(pow(3, 3) * colorLabel[i].at(j) + pow(3, 2) * colorLabel[i].at(j + 1) + + 3 * colorLabel[i].at(j + 2) + colorLabel[i].at(j + 3))]; } - - if (counter < 4) + else { - continue; + auto fix = maximas[i].size() - 4; + auto index = j - maximas[i].size() + 4; + position = map[int(pow(3, 3) * colorLabel[i].at(fix) + pow(3, 2) * colorLabel[i].at(fix + 1) + + 3 * colorLabel[i].at(fix + 2) + colorLabel[i].at(fix + 3))] + 14.0 * index; } - - for (auto c = 0; c < counter - 3; c++) + + Mat matrix = Mat::zeros(cv::Size(3, 3), CV_32FC1); + matrix.row(0) = Hc1(Rect(0, 2, 3, 1)) * (maximas[i][j]) - Hc1(Rect(0, 0, 3, 1)); + matrix.row(1) = Hc1(Rect(0, 2, 3, 1)) * (float(i + minX)) - Hc1(Rect(0, 1, 3, 1)); + matrix.row(2) = Hp2(Rect(0, 2, 3, 1)) * position - Hp2(Rect(0, 0, 3, 1)); + Mat tang = Mat::zeros(cv::Size(3, 1), CV_32FC1); + Mat b = Mat::zeros(cv::Size(1, 3), CV_32FC1); + b.row(0) = Hc1.at(0, 3) - Hc1.at(2, 3) * (maximas[i][j]); + b.row(1) = Hc1.at(1, 3) - Hc1.at(2, 3) * (float(i + minX)); + b.row(2) = Hp2.at(0, 3) - Hp2.at(2, 3) * position; + solve(matrix, b, tang); + + if (tang.at(2, 0) > 750 && tang.at(2, 0) < 1500) { - Mat matrix = Mat::zeros(cv::Size(3, 3), CV_32FC1); - matrix.row(0) = Hc1.row(2) * (rowPosition.at(0, c) + 1) - Hc1.row(0); - matrix.row(1) = Hc1.row(2) * (column + 1) - Hc1.row(1); + coordinate.push_back(tang.t()); - auto p1 = rowPosition.at(0, c); - auto p2 = rowPosition.at(0, c + 1); - auto p3 = rowPosition.at(0, c + 2); - auto p4 = rowPosition.at(0, c + 3); - - auto id = map.step[0] * featurePoint.at(column, p1) - + map.step[1] * featurePoint.at(column, p2) - + map.step[2] * featurePoint.at(column, p3) - + map.step[3] * featurePoint.at(column, p4); - - matrix.row(2) = Hp2.row(2) * (*reinterpret_cast(map.data + id) - - Hp2.row(0)); - Mat tang = Mat::zeros(cv::Size(3, 1), CV_32FC1); - Mat tmpMat = Mat::zeros(cv::Size(3, 1), CV_32FC1); - tmpMat.row(0) = Hc1.row(0) - Hc1.row(2) * (rowPosition.at(0, c) + 1); - tmpMat.row(1) = Hc1.row(1) - Hc1.row(2) * (column + 1); - tmpMat.row(2) = Hp2.row(0) - Hp2.at(2, 3) * (*reinterpret_cast(map.data + id)); - tmpMat = tmpMat.t(); - solve(matrix, tmpMat, tang); - - if (tang.at(2, 0) > 750 && tang.at(2, 0) < 1500) + int r = (int)rgbChannel[2].at(i + minX, maximas[i][j]), + g = rgbChannel[1].at(i + minX, maximas[i][j]), + b = rgbChannel[0].at(i + minX, maximas[i][j]); + int rgb = ((int)r << 16 | (int)g << 8 | (int)b); + float frgb = *reinterpret_cast(&rgb); + color.push_back(frgb); + } + // if (i == 200)cout << maximas[i][j] << "," << 0 << "," << position << endl; + if (phases[i].empty())continue; + auto pi = false; + auto start = minimas[i][0]; + if (start > maximas[i][j]) continue; + if (j == 0) + { + for (auto k = mark; k + start < maximas[i][j]; k++) { - coordinate.row(numOfCoord) = tang.t(); - numOfCoord++; + if ((start + k) < maximas[i][j] && phases[i][k] < 0)continue; + if ((start + k) < maximas[i][j] && phases[i][k] > 0) + { + if (maximas[i][j] - (start + k) < 1) + { + continue; + } + mark = k + 1; + } + else if ((start + k) > maximas[i][j]) break; } } + + for (auto k = mark; k < phases[i].size()-1; k++) + { + mark++; + double newPosition; + if ((start + k) < maximas[i][j] && phases[i][k] < 0) newPosition = position + phases[i][k]; + else if ((maximas[i][j] - (start + k)) > 1 && phases[i][k] > 0) + newPosition = position + phases[i][k] - 7; + else if ((start + k) > maximas[i][j] && phases[i][k] > 0)newPosition = position + phases[i][k]; + else if (((start + k) - maximas[i][j]) > 1 && phases[i][k] < 0) + newPosition = position + phases[i][k] + 7; + else continue; + + matrix.row(0) = Hc1(Rect(0, 2, 3, 1)) * (start + k) - Hc1(Rect(0, 0, 3, 1)); + matrix.row(2) = Hp2(Rect(0, 2, 3, 1)) * newPosition - Hp2(Rect(0, 0, 3, 1)); + b.row(0) = Hc1.at(0, 3) - Hc1.at(2, 3) * (start + k); + b.row(2) = Hp2.at(0, 3) - Hp2.at(2, 3) * newPosition; + solve(matrix, b, tang); + if (tang.at(2, 0) > 750 && tang.at(2, 0) < 1500) + { + coordinate.push_back(tang.t()); + int r = (int)rgbChannel[2].at(i + minX, (start + k)), + g = rgbChannel[1].at(i + minX, (start + k)), + b = rgbChannel[0].at(i + minX, (start + k)); + int rgb = ((int)r << 16 | (int)g << 8 | (int)b); + float frgb = *reinterpret_cast(&rgb); + color.push_back(frgb); + } + + if ((start + k) > maximas[i][j] && !pi && phases[i][k] > 0) pi = true; + + if ((start + k) > maximas[i][j] && phases[i][k] < 0 && phases[i][k + 1] > 0 && pi)break; + + } + } } - return coordinate; } -template -size_t CoreAlgorithm::argmin(ForwardIterator first, ForwardIterator last) -{ - return std::distance(first, std::min_element(first, last)); -} - - -template -size_t CoreAlgorithm::argmax(ForwardIterator first, ForwardIterator last) -{ - return std::distance(first, std::max_element(first, last)); -} - - -template -vector CoreAlgorithm::convertMat2Vector(const cv::Mat& mat) -{ - return (vector)(mat.reshape(1, 1)); //通道数不变，按行转为一行 -} void CoreAlgorithm::run() { - tmp = OSTU(image); - auto min = false; - for (auto i = 0; i < 1024; i++) + Mat mask = Mat::zeros(Size(cols, rows), CV_32FC1); + for (auto i = 0; i < rows; i++) { - for (auto j = 0; j < 1280; j++) + for (auto j = 0; j < cols; j++) { - if (tmp.at(i, j)[0] == 255) + mask.at(i, j) = (int)rgbChannel[0].at(i, j) > (int)rgbChannel[1].at(i, j) + ? ( + (int)rgbChannel[0].at(i, j) > (int)rgbChannel[2].at(i, j) + ? (int)rgbChannel[0].at(i, j) + : (int)rgbChannel[2].at(i, j)) + : ( + (int)rgbChannel[1].at(i, j) > (int)rgbChannel[2].at(i, j) + ? (int)rgbChannel[1].at(i, j) + : (int)rgbChannel[2].at(i, j)); + } + } + + tmp = OtsuAlgThreshold(mask); + + auto kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5)); + morphologyEx(tmp, tmp, MORPH_OPEN, kernel); + auto min = false; + for (auto i = 0; i < rows; i++) + { + for (auto j = 0; j < cols; j++) + { + if (tmp.at(i, j) == 255) { if (!min) { minX = i; + minY = j; min = true; } @@ -229,117 +306,207 @@ void CoreAlgorithm::run() } } + minX -= 50; + minY -= 50; + maxX += 50; + maxY += 50; - Mat grayImage = Mat::zeros(cv::Size(1024, 1280), CV_32FC1); - for (auto i = 0; i < 1024; i++) - { - for (auto j = 0; j < 1280; j++) - { - grayImage.at(i, j) = 0.2989 * channel.at(2).at(i, j) + - 0.5907 * channel.at(1).at(i, j) + - 0.1140 * channel.at(0).at(i, j); - } - } + // cout << minX << "," << minY; + // cout << maxX << "," << maxY; + // cout<(temp); - - const auto tmpPosition1 = argmax(v.begin(), v.end()); - const auto tmpPosition2 = argmin(v.begin(), v.end()); - const auto position1 = static_cast(tmpPosition1); - const auto position2 = static_cast(tmpPosition2); - - if (position1 == neighborhood) - { - featurePoint.at(i, j) = 1; - } - if (position2 == neighborhood) - { - featurePoint.at(i, j) = -1; - } + img.at(i, j) = 0.2989 * (int)rgbChannel.at(2).at(i, j) + + 0.5907 * (int)rgbChannel.at(1).at(i, j) + + 0.1140 * (int)rgbChannel.at(0).at(i, j); } } + kernel = getStructuringElement(MORPH_RECT, cv::Size(3, 3)); + morphologyEx(img, img, MORPH_CLOSE, kernel); + + GaussianBlur(img, img, Size(5, 5), 0, 0); + + Mat derivative1 = Mat::zeros(Size(cols, rows), CV_32FC1); + Mat derivative2 = Mat::zeros(Size(cols, rows), CV_32FC1); + + for (auto i = 0; i < rows; i++) + { + for (auto j = 1; j < cols - 1; j++) + { + derivative1.at(i, j) = img.at(i, j + 1) - img.at(i, j); + derivative2.at(i, j) = img.at(i, j + 1) + img.at(i, j - 1) - 2 * img.at(i, j); + } + } + + vector> maximas(0, vector(0, 0)); + vector> minimas(0, vector(0, 0)); + vector> colorLabel(0, vector(0, 0)); for (auto i = minX; i < maxX; i++) { - Mat temp = Mat::zeros(cv::Size(100, 1), CV_32FC1); - auto num = 0; + maximas.resize(i - minX + 1); + minimas.resize(i - minX + 1); + colorLabel.resize(i - minX + 1); + vector tmpMin; for (auto j = minY; j < maxY; j++) { - if (featurePoint.at(i, j) == 1) + // cout << i << endl; + if (derivative1.at(i, j) > 0 && derivative1.at(i, j + 1) < 0) { - temp.at(num, 0) = j; - int rh = channel.at(2).at(i, j); - int gh = channel.at(1).at(i, j); - int bh = channel.at(0).at(i, j); - - if (rh == gh || gh == bh || rh == bh) + double k = derivative1.at(i, j + 1) - derivative1.at(i, j); + double b = derivative1.at(i, j) - k * j; + double zero = -b / k; + double k2 = derivative2.at(i, j + 1) - derivative2.at(i, j); + double b2 = derivative2.at(i, j) - k2 * j; + double value = k2 * zero + b2; + if (value < 0 && lab.at(i, zero)[0] > 5) + { + maximas[i - minX].push_back(zero); + if (lab.at(i, zero)[2] < 126) + { + colorLabel[i - minX].push_back(2); //blue + } + else + { + if (lab.at(i, zero)[1] >= 128) + { + colorLabel[i - minX].push_back(0); //red + } + else + { + colorLabel[i - minX].push_back(1); //green + } + } + } + } + + if (derivative1.at(i, j) < 0 && derivative1.at(i, j + 1) > 0) + { + double k = derivative1.at(i, j + 1) - derivative1.at(i, j); + double b = derivative1.at(i, j) - k * j; + double zero = -b / k; + double k2 = derivative2.at(i, j + 1) - derivative2.at(i, j); + double b2 = derivative2.at(i, j) - k2 * j; + double value = k2 * zero + b2; + if (value > 0) + { + tmpMin.push_back(zero); + } + } + } + if (!tmpMin.empty() && !maximas[i - minX].empty()) + { + auto pos = 0; + for (auto j = 0; j < tmpMin.size()-1; j++) + { + + if (tmpMin[j + 1] < maximas[i - minX][pos]) { - featurePoint.at(i, j) = -2; continue; } - - auto h = Hsv(rh, gh, bh); - auto result = max_element(begin(h), end(h)); - auto nPos = static_cast(max_element(h.begin(), h.end()) - (h.begin())); - - if (nPos == 0) - { - featurePoint.at(i, j) = 0; - } - else if (nPos == 1) - { - featurePoint.at(i, j) = 1; - } - else if (nPos == 2) - { - featurePoint.at(i, j) = 2; - } - num = num + 1; - } - else if (featurePoint.at(i, j) != -1) - { - featurePoint.at(i, j) = -2; + minimas[i - minX].push_back(tmpMin[j]); + pos++; + if (pos >= maximas[i - minX].size())break; } + } } - - auto number = 0; + + vector> phases(0, vector(0, 0)); + emxArray_real_T* phase; + double x_data[1280] = {0}; + int x_size[2] = {0}; + emxInitArray_real_T(&phase, 2); + x_size[0] = 1; for (auto i = minX; i < maxX; i++) { - for (auto j = minY; j < maxY; j++) + phases.resize(i - minX + 1); + if (minimas[i - minX].empty())continue; + int start = minimas[i - minX][0]; + int end = minimas[i - minX][minimas[i - minX].size() - 1]; + x_size[1] = end - start; + for (auto j = start; j < end; j++) { - if (featurePoint.at(i, j) != -2) number++; + x_data[j - start] = (float)lab.at(i, j)[0]; + // if (i -minX== 300) + // cout<data + j) / PI * 7); + // if (i - minX == 300) + // cout << j + start << "," << *(phase->data + j) << endl; + } + // if (i == 300) { + // for (auto j = 0; j < maximas[i - minX].size(); j++) { + // cout <data + int(maximas[i - minX][j] - start)) << endl; + // + // } + // } + } + auto db = DeBruijn(3, 4); + double map[76]{0}; + for (auto i = 0; i < 61; i++) + { + int index = int(pow(3, 3) * db.at(i) + pow(3, 2) * db.at(i + 1) + 3 * db.at(i + 2) + db.at(i + 3)); + map[index] = 7.5 + 14 * i; + } + + Reconstruction(maximas, minimas, colorLabel, phases, cArg->getHc(), cArg->getHp(), map); + ofstream destFile("./Data/result/result.txt", ios::out); //浠ユ枃鏈ā寮忔墦寮out.txt澶囧啓 + for (auto i = 0; i < coordinate.size(); i++) + { + if (i == coordinate.size() - 1) + { + destFile << coordinate[i].at(0, 0) << " " << coordinate[i].at(0, 1) << " " + << coordinate[i].at(0, 2); + } + else + { + destFile << coordinate[i].at(0, 0) << " " << coordinate[i].at(0, 1) << " " + << coordinate[i].at(0, 2) << endl; //鍙互鍍忕敤cout閭ｆ牱鐢╫fstream瀵硅薄 } } - auto db = Debruijn(3, 4); - - int p, q, t, u; - p = q = t = u = 3; - int sizes[] = {p, q, t, u}; - auto all = p * q * t * u; - auto d1 = new float[all]; - for (auto i = 0; i < all; i++) - { - d1[i] = i * 1; - } - - auto map = Mat(4, sizes, CV_32S, d1); - - for (auto i = 0; i < 61; i++) - { - auto id = map.step[0] * db[i] + map.step[1] * db[i + 1] - + map.step[2] * db[i + 2] + map.step[3] * db[i + 3]; - auto* stub = reinterpret_cast(map.data + id); - *stub = 7.5 + 14 * i; - } - - auto coordinate = Reconstruction(featurePoint, number, cArg->getHc(), cArg->getHp(), map); + destFile.close(); + saveCoordinate(); +} + +void CoreAlgorithm::saveCoordinate() +{ + ofstream destFile("./Data/result/result.pcd", ios::out); //浠ユ枃鏈ā寮忔墦寮out.txt澶囧啓 + destFile << "# .PCD v0.7 - Point Cloud Data file format" << endl; + destFile << "VERSION 0.7" << endl; + destFile << "FIELDS x y z rgb" << endl; + destFile << "SIZE 4 4 4 4" << endl; + destFile << "TYPE F F F F" << endl; + destFile << "COUNT 1 1 1 1" << endl; + destFile << "WIDTH " << coordinate.size() << endl; + destFile << "HEIGHT 1" << endl; + destFile << "VIEWPOINT 0 0 0 1 0 0 0" << endl; + destFile << "POINTS " << coordinate.size() << endl; + destFile << "DATA ascii" << endl; + for (auto i = 0; i < coordinate.size(); i++) + { + // cout << i << endl; + if (i == coordinate.size() - 1) + { + destFile << coordinate[i].at(0, 0) << " " << coordinate[i].at(0, 1) << " " + << coordinate[i].at(0, 2) << " " << color[i]; + } + else + { + destFile << coordinate[i].at(0, 0) << " " << coordinate[i].at(0, 1) << " " + << coordinate[i].at(0, 2) << " " << color[i] << endl; //鍙互鍍忕敤cout閭ｆ牱鐢╫fstream瀵硅薄 + } + } + destFile.close(); } diff --git a/Classes/CoreAlgorithm.h b/Classes/CoreAlgorithm.h index 48b50b8..6a553b4 100644 --- a/Classes/CoreAlgorithm.h +++ b/Classes/CoreAlgorithm.h @@ -1,7 +1,24 @@ #pragma once + #include +#include #include #include +#include +#include +#include +#include +#include +#include +#include "cwt.h" +#include "cwt_emxAPI.h" +#include "cwt_terminate.h" +#include "rt_nonfinite.h" +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" #include "CameraArguments.h" using namespace std; @@ -9,27 +26,31 @@ using namespace cv; typedef unsigned char byte; -class CoreAlgorithm -{ +# define PI acos(-1) + +class CoreAlgorithm { private: - Mat image, tmp; - vector channel,hsv; - const int neighborhood = 5; - int minX = 0, maxX = 0, minY = 1280, maxY = 0; - CameraArguments* cArg; + Mat image, tmp, hsv, lab; + vector rgbChannel, hsvChannel; + int minX = 0, maxX = 0, minY = 0, maxY = 0; + int rows, cols; + CameraArguments *cArg; + vector coordinate; + vector color; private: - CoreAlgorithm(std::string path); - ~CoreAlgorithm(); - Mat OSTU(Mat src); - vector Debruijn(int k, int n); - vector Hsv(int r,int g,int b); - Mat Reconstruction(Mat featurePoint, int num,Mat Hc1,Mat Hp2,Mat map); - template - size_t argmin(ForwardIterator first, ForwardIterator last); - template - size_t argmax(ForwardIterator first, ForwardIterator last); - template - std::vector convertMat2Vector(const cv::Mat& mat); + static Mat OtsuAlgThreshold(Mat &src); + + static vector DeBruijn(int k, int n); + + void Reconstruction(vector> maximas, vector> minimas, vector> colorLabel, + vector> phases,const Mat &Hc1, Mat Hp2, const double *map); + public: - void run(); + CoreAlgorithm(const std::string &path, CameraArguments *cArgs); + + ~CoreAlgorithm(); + + void run(); + + void saveCoordinate(); }; diff --git a/Classes/Device.cpp b/Classes/Device.cpp new file mode 100644 index 0000000..973c1d1 --- /dev/null +++ b/Classes/Device.cpp @@ -0,0 +1,47 @@ +#include "Device.h" +#include +Device* Device::instance = nullptr; + +Device::Device() +{ + vector cams = CameraPointGrey::getCameraList(); + if (cams.size() != 0) + { + hasCamera = true; + camera = Camera::NewCamera(0, cams[0].busID, triggerModeSoftware); + CameraSettings camSettings; + camSettings.shutter = 25; + camSettings.gain = 0.0; + camera->setCameraSettings(camSettings); + camera->startCapture(); + } + + projector = new ProjectorLC4500(0); + if (projector->getIsRunning())hasProjector = true; +} + +Device* Device::getInstance() +{ + if (instance == nullptr) instance = new Device(); + return instance; +} + +Camera* Device::getCamera() +{ + return camera; +} + +Projector* Device::getProjector() +{ + return projector; +} + +bool Device::getHasCamera() +{ + return hasCamera; +} + +bool Device::getHasProjector() +{ + return hasProjector; +} diff --git a/Classes/Device.h b/Classes/Device.h new file mode 100644 index 0000000..922c914 --- /dev/null +++ b/Classes/Device.h @@ -0,0 +1,28 @@ +#pragma once + +#include "Camera.h" +#include "CameraPointGrey.h" +#include "ProjectorLC4500.h" + + +class Device +{ +public: + static Device* instance; + static Device* getInstance(); + Camera* getCamera(); + Projector* getProjector(); + bool getHasCamera(); + bool getHasProjector(); + +private: + Device(); + + Camera *camera; + Projector *projector; + // variable indicate whether the camera and projector + // have been initialized or not + bool hasCamera = false; + bool hasProjector = false; + +}; diff --git a/Classes/Help.cpp b/Classes/Help.cpp index 961a10d..30c9ea5 100644 --- a/Classes/Help.cpp +++ b/Classes/Help.cpp @@ -4,10 +4,10 @@ Help::Help(QWidget *parent) : QWidget(parent) { ui.setupUi(this); - - // QTextCodec *codec = QTextCodec::codecForName("utf-8"); //不起作用 - // QTextCodec::setCodecForLocale(codec); - ui.textBrowser->setText(QString::fromUtf8("help")); // 暂时不能写显示中文 + QPixmap *pixmap = new QPixmap(":/Reconstruction/image/reconstruction/help.png"); + pixmap->scaled(ui.label->size(), Qt::KeepAspectRatio); + ui.label->setScaledContents(true); + ui.label->setPixmap(*pixmap); } Help::~Help() diff --git a/Classes/Loading.cpp b/Classes/Loading.cpp index e693595..cfb0595 100644 --- a/Classes/Loading.cpp +++ b/Classes/Loading.cpp @@ -5,7 +5,7 @@ Loading::Loading(QWidget *parent) { setStyle(); - + setWindowFlags(Qt::FramelessWindowHint); ui.label_2->hide(); // ui.progressBar->setValue(0); // QGraphicsOpacityEffect *opacityEffect = new QGraphicsOpacityEffect; @@ -14,7 +14,7 @@ Loading::Loading(QWidget *parent) // todo 加载 // ui.progressBar->setValue(100); - + device = Device::getInstance(); ui.label->hide(); ui.label_2->show(); ready2Enter = true; diff --git a/Classes/Loading.h b/Classes/Loading.h index 31351b8..992718b 100644 --- a/Classes/Loading.h +++ b/Classes/Loading.h @@ -5,6 +5,7 @@ #include "Reconstruction.h" #include #include +#include "Device.h" class Loading : public QWidget { @@ -17,6 +18,7 @@ public: private: Ui::Loading ui; bool ready2Enter = false; + Device* device; int currentValue = 0; void updateSlot(); void setStyle(); diff --git a/Classes/MyThread.cpp b/Classes/MyThread.cpp index 5645837..2a8e0c1 100644 --- a/Classes/MyThread.cpp +++ b/Classes/MyThread.cpp @@ -13,6 +13,8 @@ void MyThread::run() io::loadPCDFile(pcd, *cloudPtr); cloud = *cloudPtr; } + + } void MyThread::setPcd(QString str) diff --git a/Classes/MyThread.h b/Classes/MyThread.h index f977d77..5bd2437 100644 --- a/Classes/MyThread.h +++ b/Classes/MyThread.h @@ -3,6 +3,7 @@ #include #include #include + using namespace std; using namespace pcl; diff --git a/Classes/Projector.h b/Classes/Projector.h new file mode 100644 index 0000000..6a97cb3 --- /dev/null +++ b/Classes/Projector.h @@ -0,0 +1,25 @@ +#ifndef PROJECTOR_H +#define PROJECTOR_H + +#include +#include + +// Abstract Projector base class +class Projector { + public: + // Interface function + Projector(){} + // Define preset pattern sequence + virtual void setPattern(unsigned int patternNumber , const unsigned char *tex , unsigned int texWidth , unsigned int texHeight) = 0; + virtual void displayPattern(unsigned int patternNumber) = 0; + // Upload and display pattern on the fly + virtual void displayTexture(const unsigned char *tex , unsigned int width , unsigned int height) = 0; + // Monochrome color display + virtual void displayBlack() = 0; + virtual void displayWhite() = 0; + virtual void getScreenRes(unsigned int *nx , unsigned int *ny) = 0; + virtual bool getIsRunning() = 0; + virtual ~Projector(){} +}; + +#endif diff --git a/Classes/ProjectorLC4500.cpp b/Classes/ProjectorLC4500.cpp new file mode 100644 index 0000000..05b8e16 --- /dev/null +++ b/Classes/ProjectorLC4500.cpp @@ -0,0 +1,116 @@ +#include "ProjectorLC4500.h" + +#include + +#include "API.h" +#include "usb.h" + +void showError(std::string err) +{ + std::cerr << "lc4500startup: " << err.c_str() << std::endl; +} + + +ProjectorLC4500::ProjectorLC4500(unsigned int): nPatterns(0), isRunning(false) +{ + std::cout << "ProjectorLC4500: preparing LightCrafter 4500 for duty... " << std::endl; + + auto mark1 = false; + auto mark2 = false; + auto mark3 = false; + auto mark4 = false; + // Initialize usb connection + if (USB_Init()) + { + showError("Could not init USB!"); + } + else + { + mark1 = true; + } + + if (USB_Open()) + { + showError("Could not connect!"); + } + else + { + mark2 = true; + } + if (!USB_IsConnected()) + { + showError("Could not connect."); + } + else + { + mark3 = true; + } + + // Make sure LC is not in standby + const bool standby = false; + if (!LCR_SetPowerMode(standby)) + { + showError("Error Setting Power Mode"); + } + else + { + mark4 = true; + } + + if (mark1 && mark2 && mark3 && mark4) isRunning = true; + LCR_SetMode(false); + LCR_SetInputSource(2, 0); + LCR_SetPixelFormat(0); +} + +void ProjectorLC4500::setPattern(unsigned int patternNumber, const unsigned char* tex, unsigned int texWidth, + unsigned int texHeight) +{ +} + +void ProjectorLC4500::displayPattern(unsigned int PatNum) +{ + LCR_LoadSplash(PatNum); + rectangle croppedArea, displayArea; + croppedArea.firstPixel = 0; + croppedArea.firstLine = 0; + croppedArea.pixelsPerLine = 912; + croppedArea.linesPerFrame = 1140; + displayArea.firstPixel = 0; + displayArea.firstLine = 0; + displayArea.pixelsPerLine = 1280; + displayArea.linesPerFrame = 800; + LCR_SetDisplay(croppedArea, displayArea); +} + +void ProjectorLC4500::displayTexture(const unsigned char* tex, unsigned int texWidth, unsigned int texHeight) +{ +} + +void ProjectorLC4500::displayBlack() +{ +} + +void ProjectorLC4500::displayWhite() +{ +} + +void ProjectorLC4500::getScreenRes(unsigned int* nx, unsigned int* ny) +{ + *nx = 912; + *ny = 1140; +} + +bool ProjectorLC4500::getIsRunning() +{ + return isRunning; +} + +ProjectorLC4500::~ProjectorLC4500() +{ + // Stop pattern sequence + LCR_PatternDisplay(0); + + USB_Close(); + USB_Exit(); +} diff --git a/Classes/ProjectorLC4500.h b/Classes/ProjectorLC4500.h new file mode 100644 index 0000000..5f8d67e --- /dev/null +++ b/Classes/ProjectorLC4500.h @@ -0,0 +1,30 @@ +#ifndef PROJECTORLC4500_H +#define PROJECTORLC4500_H + +#include +#include +#include + +#include "Projector.h" + +// Projecotr implementation for LightCrafter 4500 USB Api +class ProjectorLC4500 : public Projector { + public: + // Interface function + ProjectorLC4500(unsigned int); + // Define preset pattern sequence and upload to GPU + void setPattern(unsigned int patternNumber ,const unsigned char *tex ,unsigned int texWidth , unsigned int texHeight); + void displayPattern(unsigned int patternNumber); + // Upload and display pattern on the fly + void displayTexture(const unsigned char *tex , unsigned int width , unsigned int height); + void displayBlack(); + void displayWhite(); + void getScreenRes(unsigned int *nx , unsigned int *ny); + bool getIsRunning(); + ~ProjectorLC4500(); + private: + unsigned int nPatterns; + bool isRunning; +}; + +#endif diff --git a/Classes/Reconstruction.cpp b/Classes/Reconstruction.cpp index 45add62..c29fbba 100644 --- a/Classes/Reconstruction.cpp +++ b/Classes/Reconstruction.cpp @@ -1,12 +1,15 @@ #include "Reconstruction.h" -Reconstruction::Reconstruction(QWidget *parent) +Reconstruction::Reconstruction(QWidget* parent) : QMainWindow(parent) { ui.setupUi(this); + calibData = new CalibrationData(); + calibrator = new Calibrator(); + dirModel = new TreeModel(this); t = new MyThread; connect(t, SIGNAL(finished()), this, SLOT(setCloud())); - + device = Device::getInstance(); setStyle(); } @@ -27,11 +30,11 @@ void Reconstruction::setStyle() file.close(); QPalette palette1; - palette1.setColor(QPalette::Background, qRgba(62, 71, 128, 100)); //左侧 + palette1.setColor(QPalette::Background, qRgba(62, 71, 128, 100)); //左侧 ui.widget->setPalette(palette1); QPalette palette2; - palette2.setColor(QPalette::Background, qRgba(255, 255, 255, 100)); //白色 + palette2.setColor(QPalette::Background, qRgba(255, 255, 255, 100)); //白色 // palette2.setColor(QPalette::Background, qRgba(204, 213, 240, 100)); //背景 ui.stackedWidget->setPalette(palette2); @@ -39,12 +42,32 @@ void Reconstruction::setStyle() setPicStyle(); setButtonStyle(); } + void Reconstruction::setPicStyle() { ui.label_9->setPixmap(QPixmap(":/icon/image/reconstruction/loading.png")); - ui.label_11->setPixmap(QPixmap(":/icon/image/calibration/novideo.png")); + if (device->getHasCamera()) + { + auto w = ui.label_11->width(); + auto h = ui.label_11->height(); + Mat blackImg(h, w, CV_8UC1, cv::Scalar::all(0)); + QImage qimage = cvtools::cvMat2qImage(blackImg); + QPixmap pixmap = QPixmap::fromImage(qimage); + ui.label_11->setPixmap(pixmap.scaled(w, h, Qt::KeepAspectRatio)); + liveViewTimer = startTimer(100); + } + else + { + ui.label_11->setPixmap(QPixmap(":/icon/image/calibration/novideo.png")); + ui.pushButton_5->setEnabled(false); + ui.pushButton_6->setEnabled(false); + ui.pushButton_7->setEnabled(false); + ui.pushButton_8->setEnabled(false); + } + ui.label_21->setPixmap(QPixmap(":/icon/image/projection/novideo.jpg")); } + void Reconstruction::setButtonStyle() { QString buttonStyle = "QPushButton{background-color:white;color: black;}" @@ -73,12 +96,17 @@ void Reconstruction::setButtonStyle() ui.pushButton_15->setIcon(QIcon(":/icon/image/reconstruction/save2.png")); ui.pushButton_16->setIcon(QIcon(":/icon/image/reconstruction/color.png")); } -#pragma endregion +#pragma endregion #pragma region 界面菜单 void Reconstruction::on_pushButton_clicked() { ui.stackedWidget->setCurrentIndex(0); + // imgCount = 0; + // dirModel = new TreeModel(this); + // dirModel->clear(); + // ui.treeView->setModel(dirModel); + // ui.stackedWidget->setCurrentIndex(0); } void Reconstruction::on_pushButton_2_clicked() @@ -89,16 +117,17 @@ void Reconstruction::on_pushButton_2_clicked() void Reconstruction::on_pushButton_3_clicked() { ui.stackedWidget->setCurrentIndex(2); - updateQVTK(cloud); - if(loadingStatus) + updateQVTK(cloud, color); + if (loadingStatus) { ui.label_9->setVisible(true); - }else + } + else { ui.label_9->setVisible(false); } } -#pragma endregion +#pragma endregion #pragma region 多线程 void Reconstruction::setCloud() @@ -106,9 +135,10 @@ void Reconstruction::setCloud() ui.label_9->setVisible(false); loadingStatus = false; cloud = t->getCloud(); - updateQVTK(cloud); + updateQVTK(cloud, color); } -void Reconstruction::updateQVTK(PointCloud cloud) + +void Reconstruction::updateQVTK(PointCloud cloud, QColor color) { boost::shared_ptr viewer(new visualization::PCLVisualizer("3D Viewer")); viewer->setBackgroundColor(0.458, 0.529, 0.844); @@ -117,13 +147,18 @@ void Reconstruction::updateQVTK(PointCloud cloud) { PointCloud::Ptr cloudPtr(new PointCloud); cloudPtr = cloud.makeShared(); - viewer->addPointCloud(cloudPtr, "cloud"); + + int x = int(color.redF() * 255); + int y = int(color.greenF() * 255); + int z = int(color.blueF() * 255); + visualization::PointCloudColorHandlerCustom cloud_color(cloudPtr, x, y, z);// 统一处理点云颜色 + viewer->addPointCloud(cloudPtr, cloud_color, "cloud"); viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); } ui.qvtkWidget->SetRenderWindow(viewer->getRenderWindow()); ui.qvtkWidget->update(); } -#pragma endregion +#pragma endregion #pragma region 系统标定-按钮 @@ -132,14 +167,15 @@ void Reconstruction::on_pushButton_5_clicked() { QString fileName = QFileDialog::getOpenFileName( this, tr("open multiple image file"), - "./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)")); // todo 文件类型待确认 + "./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)")); // todo 文件类型待确认 if (fileName.isEmpty()) { QMessageBox mesg; mesg.warning(this, "WARNING", "Failed to open picture"); return; - }else + } + else { calPath = fileName; } @@ -148,26 +184,96 @@ void Reconstruction::on_pushButton_5_clicked() // 相机拍摄 void Reconstruction::on_pushButton_6_clicked() { - if(calPath.isEmpty()) + killTimer(liveViewTimer); + ui.pushButton_5->setEnabled(false); + ui.pushButton_6->setEnabled(false); + ui.pushButton_7->setEnabled(false); + ui.pushButton_8->setEnabled(false); + + char path[100]; + sprintf(path, "calib%.2d", imgCount); + fstools::mkDir(tr("./config/calib"), tr(path)); + + device->getProjector()->displayPattern(0); + //QTest::qSleep(500); + for (unsigned int i = 0; i < 44; i++) { - QMessageBox mesg; - mesg.warning(this, "WARNING", "Haven't uploaded calibration pictures!"); - }else - { - // todo 相机拍摄，存储照片集 + // Project pattern + device->getProjector()->displayPattern(i); + //QTest::qSleep(200); + // Effectuate sleep (necessary with some camera implementations) + QApplication::processEvents(); + + // Aquire frame + auto frame = device->getCamera()->getFrame(); + auto frameCV = cvtools::camFrame2Mat(frame); + cvtColor(frameCV, frameCV, cv::COLOR_BGR2GRAY); + //parameter + std::cout << cv::format("./config/calib/calib%.2d/%.2d.bmp", imgCount, i) << std::endl; + imwrite(cv::format("./config/calib/calib%.2d/%.2d.bmp", imgCount, i), frameCV); + + auto qimage = cvtools::cvMat2qImage(frameCV); + auto w = ui.label_11->width(); + auto h = ui.label_11->height(); + auto pixmap = QPixmap::fromImage(qimage); + ui.label_11->setPixmap(pixmap.scaled(w, h, Qt::KeepAspectRatio)); } + + imgCount++; + liveViewTimer = startTimer(100); + //parameter + setDirModel(tr("./config/calib")); + ui.treeView->setModel(dirModel); + + ui.pushButton_5->setEnabled(true); + ui.pushButton_6->setEnabled(true); + ui.pushButton_7->setEnabled(true); + ui.pushButton_8->setEnabled(true); + // if (calPath.isEmpty()) + // { + // QMessageBox mesg; + // mesg.warning(this, "WARNING", "Haven't uploaded calibration pictures!"); + // } + // else + // { + // // todo 相机拍摄，存储照片集 + // } // 标定日志：显示拍摄照片集存储路径 - ui.textBrowser_7->append(""); + // ui.textBrowser_7->append(""); } // 相机标定 void Reconstruction::on_pushButton_7_clicked() { - int size = ui.spinBox->value(); - int row = ui.spinBox_2->value(); - int col = ui.spinBox_3->value(); - // todo 相机标定 + auto size = ui.spinBox->value(); + auto row = ui.spinBox_2->value(); + auto col = ui.spinBox_3->value(); + calibrator->setCornerSize(size); + calibrator->setBoardRows(row); + calibrator->setBoardCols(col); + // todo 相机标定 + killTimer(liveViewTimer); + calibrator->reset(); + + std::vector imgList; + for (auto i = 0; i < dirModel->rowCount(); i++) + { + auto parent = dirModel->index(i, 0); + auto imgNum = dirModel->rowCount(parent); + for (auto j = 0; j < imgNum; j++) + { + imgList.push_back(getImage(i, j, GrayImageRole)); + } + calibrator->addFrameSequence(imgList); + imgList.clear(); + } + calibData = calibrator->calibrate(); + std::cout << "calibration success!!!" << std::endl; + calibData->print(); + ui.pushButton_6->setEnabled(true); + + if (device->getCamera()->isConnecting()) liveViewTimer = startTimer(100); // 相机参数栏：显示标定结果 ui.textBrowser->append(""); @@ -180,13 +286,13 @@ void Reconstruction::on_pushButton_7_clicked() void Reconstruction::on_pushButton_8_clicked() { QFileDialog fileDialog; - QString fileName = fileDialog.getSaveFileName(this, "Open File", "", "Text File(*.txt)"); // todo 更改文件类型 + QString fileName = fileDialog.getSaveFileName(this, "Open File", "", "Text File(*.txt)"); // todo 更改文件类型 if (fileName.isEmpty()) { return; } QFile file(fileName); - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) // todo 更改文件类型 + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) // todo 更改文件类型 { QMessageBox::warning(this, "error", "open file failure!"); return; @@ -194,9 +300,170 @@ void Reconstruction::on_pushButton_8_clicked() else { // todo 保存结果 + QString selectedFilter; + QString fileName = QFileDialog::getSaveFileName(this, QString::fromLocal8Bit("保存标定文件"), QString(), "*.xml", + &selectedFilter); + calibData->saveXML(fileName); } } -#pragma endregion + +void Reconstruction::closeEvent(QCloseEvent*) +{ + killTimer(liveViewTimer); + delete calibData; + delete calibrator; + delete dirModel; +} + +void Reconstruction::timerEvent(QTimerEvent* event) +{ + if (event->timerId() != liveViewTimer) + { + std::cerr << "Something fishy..." << std::endl << std::flush; + return; + } + + auto frame = device->getCamera()->getFrame(); + + auto frameCV = cvtools::camFrame2Mat(frame); + cvtColor(frameCV, frameCV, cv::COLOR_BGR2GRAY); + if (frameCV.rows == 0 || frameCV.cols == 0) + { + return; + } + + frameCV = frameCV.clone(); + auto qimage = cvtools::cvMat2qImage(frameCV); + + // correct size only if label has no borders/frame! + auto w = ui.label_11->width(); + auto h = ui.label_11->height(); + + auto pixmap = QPixmap::fromImage(qimage); + ui.label_11->setPixmap(pixmap.scaled(w, h, Qt::KeepAspectRatio)); +} + +void Reconstruction::setDirModel(const QString& dirname) +{ + QDir root_dir(dirname); + + //reset internal data + dirModel->clear(); + + auto dirlist = root_dir.entryList(QDir::Dirs | QDir::NoDotAndDotDot, QDir::Name); + foreach(const QString & item, dirlist) + { + QDir dir(root_dir.filePath(item)); + + QStringList filters; + filters << "*.jpg" << "*.bmp" << "*.png"; + + auto filelist = dir.entryList(filters, QDir::Files, QDir::Name); + auto path = dir.path(); + + //setup the model + auto filecount = filelist.count(); + + if (filecount < 1) + { + //no images skip + continue; + } + + unsigned row = dirModel->rowCount(); + if (!dirModel->insertRow(row)) + { + std::cout << "Failed model insert " << item.toStdString() << "(" << row << ")" << std::endl; + continue; + } + + // add the childrens + auto parent = dirModel->index(row, 0); + dirModel->setData(parent, item, Qt::DisplayRole); + + for (auto i = 0; i < filecount; i++) + { + const auto& filename = filelist.at(i); + if (!dirModel->insertRow(i, parent)) + { + std::cout << "Failed model insert " << filename.toStdString() << "(" << row << ")" << std::endl; + break; + } + + auto index = dirModel->index(i, 0, parent); + dirModel->setData(index, QString("#%1 %2").arg(i, 2, 10, QLatin1Char('0')).arg(filename), Qt::DisplayRole); + + //additional data + dirModel->setData(index, path + "/" + filename, ImageFilenameRole); + } + } + + if (dirModel->rowCount() >= 3) + { + ui.pushButton_7->setEnabled(true); + } + else + { + ui.pushButton_7->setEnabled(false); + } +} + +Mat Reconstruction::getImage(unsigned level, unsigned n, Role role) +{ + if (role != GrayImageRole && role != ColorImageRole) + { + //invalid args + return Mat(); + } + + //try to load + if (dirModel->rowCount() < static_cast(level)) + { + //out of bounds + return cv::Mat(); + } + auto parent = dirModel->index(level, 0); + if (dirModel->rowCount(parent) < static_cast(n)) + { + //out of bounds + return Mat(); + } + + auto index = dirModel->index(n, 0, parent); + if (!index.isValid()) + { + //invalid index + return Mat(); + } + + auto filename = dirModel->data(index, ImageFilenameRole).toString(); + // std::cout << "[" << (role==GrayImageRole ? "gray" : "color") << "] Filename: " << filename.toStdString() << std::endl; + + //load image + auto rgb_image = imread(filename.toStdString()); + + + if (rgb_image.rows > 0 && rgb_image.cols > 0) + { + //color + if (role == ColorImageRole) + { + return rgb_image; + } + + //gray scale + if (role == GrayImageRole) + { + Mat gray_image; + cvtColor(rgb_image, gray_image, cv::COLOR_BGR2GRAY); + return gray_image; + } + } + + return Mat(); +} + +#pragma endregion #pragma region 三维重建-按钮 @@ -206,7 +473,7 @@ void Reconstruction::on_pushButton_4_clicked() // 选择投影图案 QString fileName = QFileDialog::getOpenFileName( this, tr("open image file"), - "./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)")); + "./", tr("Image files(*.bmp *.jpg *.pbm *.pgm *.png *.ppm *.xbm *.xpm);;All files (*.*)")); if (fileName.isEmpty()) { @@ -224,8 +491,8 @@ void Reconstruction::on_pushButton_4_clicked() void Reconstruction::on_pushButton_9_clicked() { // todo 相机拍照 - picPath = "Resources/image/test.png"; // 该行仅做测试使用 - DisplayPic *picDlg = new DisplayPic(); + picPath = "Resources/image/test.png"; // 该行仅做测试使用 + DisplayPic* picDlg = new DisplayPic(); picDlg->setPicPath(picPath); connect(picDlg, SIGNAL(getPicAction(QString)), this, SLOT(setPicAction(QString))); picDlg->show(); @@ -233,11 +500,12 @@ void Reconstruction::on_pushButton_9_clicked() void Reconstruction::setPicAction(QString action) { - if(action=="confirmed") + if (action == "confirmed") { qDebug("confirmed"); confirmPic = true; - }else if(action=="canceled") + } + else if (action == "canceled") { qDebug("canceled"); confirmPic = false; @@ -248,28 +516,30 @@ void Reconstruction::setPicAction(QString action) void Reconstruction::on_pushButton_10_clicked() { // todo 保存照片 - if(confirmPic) + if (confirmPic) { QString fileName = QFileDialog::getSaveFileName(this, - tr("save image"), - "", - tr("*.png;; *.jpg;; *.bmp;; All files(*.*)")); + tr("save image"), + "", + tr("*.png;; *.jpg;; *.bmp;; All files(*.*)")); if (!fileName.isNull()) { - QImage iim(picPath);//创建一个图片对象,存储源图片 - QPainter painter(&iim);//设置绘画设备 - QFile file(fileName);//创建一个文件对象，存储用户选择的文件 - if (!file.open(QIODevice::ReadWrite)) { - return; + QImage iim(picPath); //创建一个图片对象,存储源图片 + QPainter painter(&iim); //设置绘画设备 + QFile file(fileName); //创建一个文件对象，存储用户选择的文件 + if (!file.open(QIODevice::ReadWrite)) + { + return; } QByteArray ba; QBuffer buffer(&ba); buffer.open(QIODevice::WriteOnly); - iim.save(&buffer, "JPG");//把图片以流方式写入文件缓存流中 - file.write(ba);//将流中的图片写入文件对象当中 + iim.save(&buffer, "JPG"); //把图片以流方式写入文件缓存流中 + file.write(ba); //将流中的图片写入文件对象当中 } - }else + } + else { QMessageBox mesg; mesg.warning(this, "WARNING", "Haven't taken picture!"); @@ -280,9 +550,52 @@ void Reconstruction::on_pushButton_10_clicked() void Reconstruction::on_pushButton_17_clicked() { // todo 开始重建 + // auto cArg = CameraArguments::getInstance(); + Mat r(3, 3, CV_32F); + double m0[3][3] = { + {9.7004457782050868e-001, 1.3447278830863673e-002, 2.4255450466457243e-001}, + {-8.7082927494022376e-003, 9.9974988338843274e-001, -2.0599424802792338e-002}, + {-2.4277084396282392e-001, 1.7870124701864658e-002, 9.6991905639837694e-001} + }; + for (auto i = 0; i < r.rows; i++) + for (auto j = 0; j < r.cols; j++) + r.at(i, j) = m0[i][j]; + + Mat t(1, 3, CV_32F); + double m1[1][3] = { + {-1.9511179496234658e+002, 1.2627509817628756e+001, -5.9345885017522171e+001} + }; + + for (auto i = 0; i < t.rows; i++) + for (auto j = 0; j < t.cols; j++) + t.at(i, j) = m1[i][j]; + + Mat kc(3, 3, CV_32F); + double m2[3][3] = { + {2.1536653255083029e+003, 0., 6.1886776197116581e+002}, + {0., 2.1484363899666910e+003, 5.0694898820460787e+002}, + {0., 0., 1.} + }; + for (auto i = 0; i < kc.rows; i++) + for (auto j = 0; j < kc.cols; j++) + kc.at(i, j) = m2[i][j]; + + Mat kp(3, 3, CV_32F); + double m3[3][3] = { + {1.7235093158297350e+003, 0., 4.4128195628736904e+002}, + {0., 3.4533404000869359e+003, 5.7316457428558715e+002}, + {0., 0., 1.} + }; + for (auto i = 0; i < kp.rows; i++) + for (auto j = 0; j < kp.cols; j++) + kp.at(i, j) = m3[i][j]; + + auto cArg = CameraArguments::getInstance(r, t, kc, kp); + CoreAlgorithm testCase = CoreAlgorithm("./Data/image/reconstruction/tq.png", cArg); + testCase.run(); } -#pragma endregion +#pragma endregion #pragma region 点云渲染-按钮 @@ -301,7 +614,7 @@ void Reconstruction::on_pushButton_12_clicked() // 导入点云 void Reconstruction::on_pushButton_13_clicked() { - if(loadingStatus) + if (loadingStatus) { QMessageBox mesg; mesg.warning(this, "WARNING", "正在加载… "); @@ -309,8 +622,8 @@ void Reconstruction::on_pushButton_13_clicked() } QString fileName = QFileDialog::getOpenFileName( - this, tr("open multiple image file"), - "./", tr("PCD files(*.pcd);;All files (*.*)")); // todo 文件类型待确认 + this, tr("open multiple image file"), + "./", tr("PCD files(*.pcd);;All files (*.*)")); // todo 文件类型待确认 if (fileName.isEmpty()) { @@ -338,9 +651,9 @@ void Reconstruction::on_pushButton_14_clicked() void Reconstruction::on_pushButton_15_clicked() { QString fileName = QFileDialog::getSaveFileName(this, - tr("save screen shot"), - "", - tr("*.png;; *.jpg;; *.bmp;; All files(*.*)")); + tr("save screen shot"), + "", + tr("*.png;; *.jpg;; *.bmp;; All files(*.*)")); if (!fileName.isNull()) { @@ -353,13 +666,14 @@ void Reconstruction::on_pushButton_15_clicked() // 颜色选取 void Reconstruction::on_pushButton_16_clicked() { - color = QColorDialog::getColor(Qt::black); - if (color.isValid()){ - // qDebug("x:%f, %f, %f",color.redF(), color.greenF(), color.blueF()); - // todo 颜色选取框已选择颜色color，接下来对color进行处理 + QColor colortmp = QColorDialog::getColor(Qt::black); + if (colortmp.isValid()) { + color = colortmp; + updateQVTK(cloud, color); } } + // 帮助 void Reconstruction::on_pushButton_18_clicked() { diff --git a/Classes/Reconstruction.h b/Classes/Reconstruction.h index a2f998f..a4d0be4 100644 --- a/Classes/Reconstruction.h +++ b/Classes/Reconstruction.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "DisplayPic.h" #include "ui_Reconstruction.h" #include @@ -15,33 +16,55 @@ #include #include #include +#include "Camera.h" +#include "Projector.h" +#include "fstools.h" +#include "TreeModel.h" +#include "Calibrator.h" +#include "CalibrationData.h" +#include "Device.h" +#include "CoreAlgorithm.h" #include "MyThread.h" #include "Help.h" +#include using namespace pcl; using namespace std; +enum Role { ImageFilenameRole = Qt::UserRole, GrayImageRole, ColorImageRole }; + class Reconstruction : public QMainWindow { Q_OBJECT public: Reconstruction(QWidget *parent = Q_NULLPTR); + void timerEvent(QTimerEvent* event); + void closeEvent(QCloseEvent*); + void setDirModel(const QString& dirname); + Mat getImage(unsigned level, unsigned n, Role role); private: Ui::ReconstructionClass ui; + Device* device; QString calPath; // 系统标定：标定图像的存储路径 QString picPath = "Result/result.png"; // 三维重建：拍摄照片的存储路径 PointCloud cloud; bool confirmPic = false; // 三维重建：确定是否用所拍照片进行重建 QColor color = Qt::black; // 点云渲染：颜色 + int liveViewTimer; + TreeModel* dirModel; + CalibrationData* calibData; + Calibrator* calibrator; + int imgCount; + // 多线程 MyThread* t; bool loadingStatus = false; // 点云渲染 void setStyle(); void setPicStyle(); void setButtonStyle(); - void updateQVTK(PointCloud cloud); + void updateQVTK(PointCloud cloud, QColor color); private slots: void on_pushButton_clicked(); @@ -64,4 +87,4 @@ private slots: void on_pushButton_18_clicked(); void setPicAction(QString action); void setCloud(); -}; +}; \ No newline at end of file diff --git a/Classes/TreeModel.cpp b/Classes/TreeModel.cpp new file mode 100644 index 0000000..5b0c713 --- /dev/null +++ b/Classes/TreeModel.cpp @@ -0,0 +1,205 @@ +#include "TreeModel.h" + +TreeModel::TreeModel(QObject * parent) : + QAbstractItemModel(parent) , + _columnCount(1) , + _horizontalHeader(QList()<(index.internalPointer()); +} + +const TreeModel::Item * TreeModel::get_item(const QModelIndex & index) const +{ + return const_cast(this)->get_item(index); +} + +QModelIndex TreeModel::index(int row , int column , const QModelIndex & parent) const +{ + const Item * item = get_item(parent); + if (!item) + { + item = &_root; + } + + return createIndex(row , column , const_cast(item->child(row))); +} + +QModelIndex TreeModel::parent(const QModelIndex & index) const +{ + const Item * item = get_item(index); + if (item) + { + const Item * parent = item->parent(); + if (parent) + { + const Item * grand_parent = parent->parent(); + int row = 0; + if (grand_parent) + { + row = grand_parent->childRow(parent); + } + return createIndex(row , 0 , const_cast(parent)); + } + } + return QModelIndex(); +} + +int TreeModel::rowCount(const QModelIndex & parent) const +{ + const Item * item = get_item(parent); + if (!item) + { + item = &_root; + } + return item->childrenCount(); +} + +int TreeModel::columnCount(const QModelIndex & parent) const +{ + return _columnCount; +} + +QVariant TreeModel::data(const QModelIndex & index , int role) const +{ + const Item * item = get_item(index); + if (item) + { + return item->data(role); + } + + //not found + return QVariant(); +} + +QVariant TreeModel::headerData(int section , Qt::Orientation orientation , int role) const +{ + if (orientation==Qt::Horizontal && role==Qt::DisplayRole && section<_horizontalHeader.size()) + { + return _horizontalHeader.at(section); + } + return QVariant(); +} + +bool TreeModel::setData(const QModelIndex & index , const QVariant & value, int role) +{ + Item * item = get_item(index); + if (item) + { + item->setData(value , role); + return true; + } + + //error + return false; +} + +bool TreeModel::insertRow(int row , const QModelIndex & parent) +{ + Item * item = get_item(parent); + if (!item) + { + item = &_root; + } + + beginInsertRows(parent , row ,row); + bool rv = item->insertRow(row); + endInsertRows(); + + return rv; +} + +void TreeModel::clear(void) +{ + //removeRows(0 rowCount()); + beginResetModel(); + _root.clear(); + endResetModel(); +} + + +unsigned TreeModel::Item::next_id = 0; + +TreeModel::Item::Item() : _id(next_id++) , _parent(NULL), _data(), _children() +{ +} + +void TreeModel::Item::clear(void) +{ + _children.clear(); + _data.clear(); +} + +bool TreeModel::Item::insertRow(int row) +{ + if (row>=0 && row<=_children.size()) + { + _children.insert(row , Item()); + _children[row].setParent(this); + return true; + } + return false; +} + +void TreeModel::Item::setData(const QVariant & value, int role) +{ + _data.insert(role , value); +} + +QVariant TreeModel::Item::data(int role) const +{ + QMap::const_iterator iter = _data.constFind(role); + if (iter!=_data.constEnd()) + { + return *iter; + } + return QVariant(); +} + +int TreeModel::Item::childrenCount(void) const +{ + return _children.size(); +} + +TreeModel::Item * TreeModel::Item::child(int index) +{ + if (index>=0 && index<_children.size()) + { + return &(_children[index]); + } + return NULL; +} + +const TreeModel::Item * TreeModel::Item::child(int index) const +{ + return const_cast(this)->child(index); +} + +int TreeModel::Item::childRow(const TreeModel::Item * child) const +{ + if (!child) + { + return -1; + } + int index = 0; + foreach(const TreeModel::Item & item, _children) + { + if (item.id()==child->id()) + { + return index; + } + index++; + } + return -1; +} diff --git a/Classes/TreeModel.h b/Classes/TreeModel.h new file mode 100644 index 0000000..abe28f3 --- /dev/null +++ b/Classes/TreeModel.h @@ -0,0 +1,68 @@ +#ifndef __TREEMODEL_HPP__ +#define __TREEMODEL_HPP__ + +#include +#include + + +class TreeModel : public QAbstractItemModel +{ + class Item + { + public: + Item(); + + inline unsigned id(void) const {return _id;} + + void clear(void); + bool insertRow(int row); + + QVariant data(int role) const; + void setData(const QVariant & value, int role); + + inline Item * parent(void) const {return _parent;} + inline void setParent(Item * parent) {_parent = parent;} + + int childrenCount(void) const; + const Item * child(int index) const; + Item * child(int index); + int childRow(const Item * child) const; + + private: + unsigned _id; + Item * _parent; + QMap _data; + QList _children; + + static unsigned next_id; + }; + +public: + TreeModel(QObject * parent = 0); + TreeModel(unsigned columns, QObject * parent = 0); + + /* Read-Only model members */ + virtual QModelIndex index(int row , int column, const QModelIndex & parent = QModelIndex()) const; + virtual QModelIndex parent(const QModelIndex & index) const; + virtual int rowCount(const QModelIndex & parent = QModelIndex()) const; + virtual int columnCount(const QModelIndex & parent = QModelIndex()) const; + virtual QVariant data(const QModelIndex & index, int role) const; + virtual QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const; + + /* Editable Model members */ + bool insertRow(int row, const QModelIndex & parent = QModelIndex()); + virtual bool setData(const QModelIndex & index, const QVariant & value, int role = Qt::EditRole); + + void clear(void); + +private: + Item * get_item(const QModelIndex & index); + const Item * get_item(const QModelIndex & index) const; + +private: + unsigned _columnCount; + QList _horizontalHeader; + Item _root; +}; + +#endif //__TREEMODEL_HPP__ diff --git a/Classes/bsxfun.cpp b/Classes/bsxfun.cpp new file mode 100644 index 0000000..9e1e231 --- /dev/null +++ b/Classes/bsxfun.cpp @@ -0,0 +1,94 @@ +// +// File: bsxfun.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "bsxfun.h" +#include "cwt.h" +#include "cwt_emxutil.h" +#include "rt_nonfinite.h" +#include + +// Function Definitions + +// +// Arguments : const creal_T a_data[] +// const int a_size[2] +// const emxArray_real_T *b +// emxArray_creal_T *c +// Return Type : void +// +void bsxfun(const creal_T a_data[], const int a_size[2], const emxArray_real_T + *b, emxArray_creal_T *c) +{ + int u0; + int u1; + int csz_idx_1; + int i; + int acoef; + int bcoef; + int b_bcoef; + int i1; + int k; + double d; + u0 = b->size[1]; + u1 = a_size[1]; + if (u0 < u1) { + u1 = u0; + } + + if (b->size[1] == 1) { + csz_idx_1 = a_size[1]; + } else if (a_size[1] == 1) { + csz_idx_1 = b->size[1]; + } else if (a_size[1] == b->size[1]) { + csz_idx_1 = a_size[1]; + } else { + csz_idx_1 = u1; + } + + i = c->size[0] * c->size[1]; + c->size[0] = b->size[0]; + u0 = b->size[1]; + u1 = a_size[1]; + if (u0 < u1) { + u1 = u0; + } + + if (b->size[1] == 1) { + c->size[1] = a_size[1]; + } else if (a_size[1] == 1) { + c->size[1] = b->size[1]; + } else if (a_size[1] == b->size[1]) { + c->size[1] = a_size[1]; + } else { + c->size[1] = u1; + } + + emxEnsureCapacity_creal_T(c, i); + if ((b->size[0] != 0) && (csz_idx_1 != 0)) { + acoef = (a_size[1] != 1); + bcoef = (b->size[1] != 1); + i = csz_idx_1 - 1; + for (u0 = 0; u0 <= i; u0++) { + u1 = acoef * u0; + csz_idx_1 = bcoef * u0; + b_bcoef = (b->size[0] != 1); + i1 = c->size[0] - 1; + for (k = 0; k <= i1; k++) { + d = b->data[b_bcoef * k + b->size[0] * csz_idx_1]; + c->data[k + c->size[0] * u0].re = d * a_data[u1].re; + c->data[k + c->size[0] * u0].im = d * a_data[u1].im; + } + } + } +} + +// +// File trailer for bsxfun.cpp +// +// [EOF] +// diff --git a/Classes/bsxfun.h b/Classes/bsxfun.h new file mode 100644 index 0000000..f757506 --- /dev/null +++ b/Classes/bsxfun.h @@ -0,0 +1,27 @@ +// +// File: bsxfun.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef BSXFUN_H +#define BSXFUN_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void bsxfun(const creal_T a_data[], const int a_size[2], const + emxArray_real_T *b, emxArray_creal_T *c); + +#endif + +// +// File trailer for bsxfun.h +// +// [EOF] +// diff --git a/Classes/cvtools.cpp b/Classes/cvtools.cpp new file mode 100644 index 0000000..88beaca --- /dev/null +++ b/Classes/cvtools.cpp @@ -0,0 +1,66 @@ +#include "cvtools.h" + +#include +#include "Camera.h" + +namespace cvtools { + + QImage cvMat2qImage(cv::Mat mat){ + // 8-bits unsigned, NO. OF CHANNELS=1 + if(mat.type()==CV_8UC1) { + // Set the color table (used to tranMVate colour indexes to qRgb values) + QVector colorTable; + for (int i=0; i<256; i++) + colorTable.push_back(qRgb(i,i,i)); + // Copy input Mat + QImage img((const uchar*)mat.data, mat.cols, mat.rows, mat.step, QImage::Format_Indexed8); + img.setColorTable(colorTable); + return img; + } else if(mat.type()==CV_8UC3) { + // Copy input Mat + QImage img((const uchar*)mat.data, mat.cols, mat.rows, mat.step, QImage::Format_RGB888); + return img; + } else if(mat.type()==CV_16UC1) { + mat.convertTo(mat, CV_8UC1, 1.0/256.0); + return cvMat2qImage(mat); + } else if(mat.type()==CV_16UC3) { + mat.convertTo(mat, CV_8UC3, 1.0/256.0); + return cvMat2qImage(mat); + } else if(mat.type()==CV_32FC1) { + cv::Mat rgb(mat.size(), CV_32FC3); + rgb.addref(); + cv::cvtColor(mat, rgb, cv::COLOR_GRAY2RGB); + // Copy input Mat + QImage img((const uchar*)rgb.data, rgb.cols, rgb.rows, rgb.step, QImage::Format_RGB32); + rgb.release(); + return img; + } else { + std::cerr << "SMVideoWidget: cv::Mat could not be converted to QImage!"; + return QImage(); + } + } + + cv::Mat Matx33dToMat(cv::Matx33d mat) { + return cv::Mat(mat); + } + + cv::Mat Vec3fToMat(cv::Vec vec) { + return (cv::Mat(vec)); + } + + cv::Mat camFrame2Mat(CameraFrame camFrame) { + cv::Mat frameCV(camFrame.height, camFrame.width, CV_8UC3, camFrame.memory); + frameCV = frameCV.clone(); + + std::vector bgr, rgb; + split(frameCV, bgr); + rgb.resize(3); + rgb[0] = bgr[2]; + rgb[1] = bgr[1]; + rgb[2] = bgr[0]; + cv::merge(rgb, frameCV); + + return frameCV; + } + +} diff --git a/Classes/cvtools.h b/Classes/cvtools.h new file mode 100644 index 0000000..14a8843 --- /dev/null +++ b/Classes/cvtools.h @@ -0,0 +1,18 @@ +#ifndef CVTOOLS_H +#define CVTOOLS_H + + +#include +#include +#include "Camera.h" +// struct CameraFrame; + +namespace cvtools +{ + QImage cvMat2qImage(cv::Mat mat); + cv::Mat Matx33dToMat(cv::Matx33d mat); + cv::Mat Vec3fToMat(cv::Vec vec); + cv::Mat camFrame2Mat(CameraFrame camFrame); +} + +#endif // CVTOOLS_H diff --git a/Classes/cwt.cpp b/Classes/cwt.cpp new file mode 100644 index 0000000..f92de56 --- /dev/null +++ b/Classes/cwt.cpp @@ -0,0 +1,291 @@ +// +// File: cwt.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt.h" +#include "cwt_data.h" +#include "cwt_emxutil.h" +#include "cwt_initialize.h" +#include "cwtfilterbank.h" +#include "rt_defines.h" +#include "rt_nonfinite.h" +#include +#include +#include +#include + +// Function Declarations +static double rt_atan2d_snf(double u0, double u1); +static double rt_hypotd_snf(double u0, double u1); + +// Function Definitions + +// +// Arguments : double u0 +// double u1 +// Return Type : double +// +static double rt_atan2d_snf(double u0, double u1) +{ + double y; + int b_u0; + int b_u1; + if (rtIsNaN(u0) || rtIsNaN(u1)) { + y = rtNaN; + } else if (rtIsInf(u0) && rtIsInf(u1)) { + if (u0 > 0.0) { + b_u0 = 1; + } else { + b_u0 = -1; + } + + if (u1 > 0.0) { + b_u1 = 1; + } else { + b_u1 = -1; + } + + y = atan2(static_cast(b_u0), static_cast(b_u1)); + } else if (u1 == 0.0) { + if (u0 > 0.0) { + y = RT_PI / 2.0; + } else if (u0 < 0.0) { + y = -(RT_PI / 2.0); + } else { + y = 0.0; + } + } else { + y = atan2(u0, u1); + } + + return y; +} + +// +// Arguments : double u0 +// double u1 +// Return Type : double +// +static double rt_hypotd_snf(double u0, double u1) +{ + double y; + double a; + a = std::abs(u0); + y = std::abs(u1); + if (a < y) { + a /= y; + y *= std::sqrt(a * a + 1.0); + } else if (a > y) { + y /= a; + y = a * std::sqrt(y * y + 1.0); + } else { + if (!rtIsNaN(y)) { + y = a * 1.4142135623730951; + } + } + + return y; +} + +// +// Arguments : const double x_data[] +// const int x_size[2] +// emxArray_real_T *phase +// Return Type : void +// +void cwt(const double x_data[], const int x_size[2], emxArray_real_T *phase) +{ + cwtfilterbank fb; + int b_x_size[2]; + int loop_ub; + double b_x_data[1280]; + emxArray_creal_T *cfs; + emxArray_creal_T *max_s; + int m; + int n; + int i; + boolean_T SCALEA; + double ma; + boolean_T SCALEB; + double x; + double absbi; + double absai; + double absbr; + double Ma; + if (isInitialized_cwt == false) { + cwt_initialize(); + } + + emxInitStruct_cwtfilterbank(&fb); + cwtfilterbank_cwtfilterbank(&fb, static_cast(x_size[1])); + b_x_size[0] = 1; + b_x_size[1] = x_size[1]; + loop_ub = x_size[0] * x_size[1] - 1; + if (0 <= loop_ub) { + std::memcpy(&b_x_data[0], &x_data[0], (loop_ub + 1) * sizeof(double)); + } + + emxInit_creal_T(&cfs, 2); + emxInit_creal_T(&max_s, 2); + cwtfilterbank_wt(&fb, b_x_data, b_x_size, cfs); + m = cfs->size[0]; + n = cfs->size[1]; + i = max_s->size[0] * max_s->size[1]; + max_s->size[0] = 1; + max_s->size[1] = cfs->size[1]; + emxEnsureCapacity_creal_T(max_s, i); + if (cfs->size[1] >= 1) { + for (loop_ub = 0; loop_ub < n; loop_ub++) { + max_s->data[loop_ub] = cfs->data[cfs->size[0] * loop_ub]; + for (i = 2; i <= m; i++) { + if (rtIsNaN(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re) || rtIsNaN + (cfs->data[(i + cfs->size[0] * loop_ub) - 1].im)) { + SCALEA = false; + } else if (rtIsNaN(max_s->data[loop_ub].re) || rtIsNaN(max_s-> + data[loop_ub].im)) { + SCALEA = true; + } else { + ma = std::abs(max_s->data[loop_ub].re); + if ((ma > 8.9884656743115785E+307) || (std::abs(max_s->data[loop_ub]. + im) > 8.9884656743115785E+307)) { + SCALEA = true; + } else { + SCALEA = false; + } + + if ((std::abs(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re) > + 8.9884656743115785E+307) || (std::abs(cfs->data[(i + cfs->size[0] + * loop_ub) - 1].im) > 8.9884656743115785E+307)) { + SCALEB = true; + } else { + SCALEB = false; + } + + if (SCALEA || SCALEB) { + x = rt_hypotd_snf(max_s->data[loop_ub].re / 2.0, max_s->data[loop_ub] + .im / 2.0); + absbi = rt_hypotd_snf(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re + / 2.0, cfs->data[(i + cfs->size[0] * loop_ub) + - 1].im / 2.0); + } else { + x = rt_hypotd_snf(max_s->data[loop_ub].re, max_s->data[loop_ub].im); + absbi = rt_hypotd_snf(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re, + cfs->data[(i + cfs->size[0] * loop_ub) - 1].im); + } + + if (x == absbi) { + absai = std::abs(max_s->data[loop_ub].im); + absbr = std::abs(cfs->data[(i + cfs->size[0] * loop_ub) - 1].re); + absbi = std::abs(cfs->data[(i + cfs->size[0] * loop_ub) - 1].im); + if (ma > absai) { + Ma = ma; + ma = absai; + } else { + Ma = absai; + } + + if (absbr > absbi) { + absai = absbr; + absbr = absbi; + } else { + absai = absbi; + } + + if (Ma > absai) { + if (ma < absbr) { + x = Ma - absai; + absbi = (ma / 2.0 + absbr / 2.0) / (Ma / 2.0 + absai / 2.0) * + (absbr - ma); + } else { + x = Ma; + absbi = absai; + } + } else if (Ma < absai) { + if (ma > absbr) { + absbi = absai - Ma; + x = (ma / 2.0 + absbr / 2.0) / (Ma / 2.0 + absai / 2.0) * (ma - + absbr); + } else { + x = Ma; + absbi = absai; + } + } else { + x = ma; + absbi = absbr; + } + + if (x == absbi) { + x = rt_atan2d_snf(max_s->data[loop_ub].im, max_s->data[loop_ub].re); + absbi = rt_atan2d_snf(cfs->data[(i + cfs->size[0] * loop_ub) - 1]. + im, cfs->data[(i + cfs->size[0] * loop_ub) - + 1].re); + if (x == absbi) { + absbi = cfs->data[(i + cfs->size[0] * loop_ub) - 1].re; + absai = cfs->data[(i + cfs->size[0] * loop_ub) - 1].im; + if (x > 0.78539816339744828) { + if (x > 2.3561944901923448) { + x = -max_s->data[loop_ub].im; + absbi = -absai; + } else { + x = -max_s->data[loop_ub].re; + absbi = -absbi; + } + } else if (x > -0.78539816339744828) { + x = max_s->data[loop_ub].im; + absbi = absai; + } else if (x > -2.3561944901923448) { + x = max_s->data[loop_ub].re; + } else { + x = -max_s->data[loop_ub].im; + absbi = -absai; + } + + if (x == absbi) { + x = 0.0; + absbi = 0.0; + } + } + } + } + + SCALEA = (x < absbi); + } + + if (SCALEA) { + max_s->data[loop_ub] = cfs->data[(i + cfs->size[0] * loop_ub) - 1]; + } + } + } + } + + emxFree_creal_T(&cfs); + i = phase->size[0] * phase->size[1]; + phase->size[0] = 1; + phase->size[1] = max_s->size[1]; + emxEnsureCapacity_real_T(phase, i); + loop_ub = max_s->size[1]; + for (i = 0; i < loop_ub; i++) { + phase->data[i] = 0.0; + } + + i = max_s->size[1]; + for (loop_ub = 0; loop_ub < i; loop_ub++) { + phase->data[loop_ub] = std::atan(max_s->data[loop_ub].im / max_s-> + data[loop_ub].re); + } + + emxFree_creal_T(&max_s); + + // + emxFreeStruct_cwtfilterbank(&fb); +} + +// +// File trailer for cwt.cpp +// +// [EOF] +// diff --git a/Classes/cwt.h b/Classes/cwt.h new file mode 100644 index 0000000..04de1fc --- /dev/null +++ b/Classes/cwt.h @@ -0,0 +1,27 @@ +// +// File: cwt.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_H +#define CWT_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void cwt(const double x_data[], const int x_size[2], emxArray_real_T + *phase); + +#endif + +// +// File trailer for cwt.h +// +// [EOF] +// diff --git a/Classes/cwt_data.cpp b/Classes/cwt_data.cpp new file mode 100644 index 0000000..f0e72bf --- /dev/null +++ b/Classes/cwt_data.cpp @@ -0,0 +1,33 @@ +// +// File: cwt_data.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt_data.h" +#include "cwt.h" +#include "rt_nonfinite.h" +#include + +// Variable Definitions +omp_nest_lock_t emlrtNestLockGlobal; +const char cv1[128] = { '\x00', '\x01', '\x02', '\x03', '\x04', '\x05', '\x06', + '\x07', '\x08', ' ', '\x0a', '\x0b', '\x0c', '\x0d', '\x0e', '\x0f', '\x10', + '\x11', '\x12', '\x13', '\x14', '\x15', '\x16', '\x17', '\x18', '\x19', '\x1a', + '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ', '!', '\"', '#', '$', '%', '&', + '\'', '(', ')', '*', '+', ',', '-', '.', '/', '0', '1', '2', '3', '4', '5', + '6', '7', '8', '9', ':', ';', '<', '=', '>', '?', '@', 'a', 'b', 'c', 'd', 'e', + 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', + 'v', 'w', 'x', 'y', 'z', '[', '\\', ']', '^', '_', '`', 'a', 'b', 'c', 'd', + 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', + 'u', 'v', 'w', 'x', 'y', 'z', '{', '|', '}', '~', '\x7f' }; + +boolean_T isInitialized_cwt = false; + +// +// File trailer for cwt_data.cpp +// +// [EOF] +// diff --git a/Classes/cwt_data.h b/Classes/cwt_data.h new file mode 100644 index 0000000..2c32fd9 --- /dev/null +++ b/Classes/cwt_data.h @@ -0,0 +1,28 @@ +// +// File: cwt_data.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_DATA_H +#define CWT_DATA_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Variable Declarations +extern omp_nest_lock_t emlrtNestLockGlobal; +extern const char cv1[128]; +extern boolean_T isInitialized_cwt; + +#endif + +// +// File trailer for cwt_data.h +// +// [EOF] +// diff --git a/Classes/cwt_emxAPI.cpp b/Classes/cwt_emxAPI.cpp new file mode 100644 index 0000000..b977e95 --- /dev/null +++ b/Classes/cwt_emxAPI.cpp @@ -0,0 +1,130 @@ +// +// File: cwt_emxAPI.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt_emxAPI.h" +#include "cwt.h" +#include "cwt_emxutil.h" +#include "rt_nonfinite.h" +#include +#include + +// Function Definitions + +// +// Arguments : int numDimensions +// const int *size +// Return Type : emxArray_real_T * +// +emxArray_real_T *emxCreateND_real_T(int numDimensions, const int *size) +{ + emxArray_real_T *emx; + int numEl; + int i; + emxInit_real_T(&emx, numDimensions); + numEl = 1; + for (i = 0; i < numDimensions; i++) { + numEl *= size[i]; + emx->size[i] = size[i]; + } + + emx->data = (double *)std::calloc(static_cast(numEl), sizeof + (double)); + emx->numDimensions = numDimensions; + emx->allocatedSize = numEl; + return emx; +} + +// +// Arguments : double *data +// int numDimensions +// const int *size +// Return Type : emxArray_real_T * +// +emxArray_real_T *emxCreateWrapperND_real_T(double *data, int numDimensions, + const int *size) +{ + emxArray_real_T *emx; + int numEl; + int i; + emxInit_real_T(&emx, numDimensions); + numEl = 1; + for (i = 0; i < numDimensions; i++) { + numEl *= size[i]; + emx->size[i] = size[i]; + } + + emx->data = data; + emx->numDimensions = numDimensions; + emx->allocatedSize = numEl; + emx->canFreeData = false; + return emx; +} + +// +// Arguments : double *data +// int rows +// int cols +// Return Type : emxArray_real_T * +// +emxArray_real_T *emxCreateWrapper_real_T(double *data, int rows, int cols) +{ + emxArray_real_T *emx; + emxInit_real_T(&emx, 2); + emx->size[0] = rows; + emx->size[1] = cols; + emx->data = data; + emx->numDimensions = 2; + emx->allocatedSize = rows * cols; + emx->canFreeData = false; + return emx; +} + +// +// Arguments : int rows +// int cols +// Return Type : emxArray_real_T * +// +emxArray_real_T *emxCreate_real_T(int rows, int cols) +{ + emxArray_real_T *emx; + int numEl; + emxInit_real_T(&emx, 2); + emx->size[0] = rows; + numEl = rows * cols; + emx->size[1] = cols; + emx->data = (double *)std::calloc(static_cast(numEl), sizeof + (double)); + emx->numDimensions = 2; + emx->allocatedSize = numEl; + return emx; +} + +// +// Arguments : emxArray_real_T *emxArray +// Return Type : void +// +void emxDestroyArray_real_T(emxArray_real_T *emxArray) +{ + emxFree_real_T(&emxArray); +} + +// +// Arguments : emxArray_real_T **pEmxArray +// int numDimensions +// Return Type : void +// +void emxInitArray_real_T(emxArray_real_T **pEmxArray, int numDimensions) +{ + emxInit_real_T(pEmxArray, numDimensions); +} + +// +// File trailer for cwt_emxAPI.cpp +// +// [EOF] +// diff --git a/Classes/cwt_emxAPI.h b/Classes/cwt_emxAPI.h new file mode 100644 index 0000000..d425ecf --- /dev/null +++ b/Classes/cwt_emxAPI.h @@ -0,0 +1,32 @@ +// +// File: cwt_emxAPI.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_EMXAPI_H +#define CWT_EMXAPI_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern emxArray_real_T *emxCreateND_real_T(int numDimensions, const int *size); +extern emxArray_real_T *emxCreateWrapperND_real_T(double *data, int + numDimensions, const int *size); +extern emxArray_real_T *emxCreateWrapper_real_T(double *data, int rows, int cols); +extern emxArray_real_T *emxCreate_real_T(int rows, int cols); +extern void emxDestroyArray_real_T(emxArray_real_T *emxArray); +extern void emxInitArray_real_T(emxArray_real_T **pEmxArray, int numDimensions); + +#endif + +// +// File trailer for cwt_emxAPI.h +// +// [EOF] +// diff --git a/Classes/cwt_emxutil.cpp b/Classes/cwt_emxutil.cpp new file mode 100644 index 0000000..d213908 --- /dev/null +++ b/Classes/cwt_emxutil.cpp @@ -0,0 +1,301 @@ +// +// File: cwt_emxutil.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt_emxutil.h" +#include "cwt.h" +#include "rt_nonfinite.h" +#include +#include +#include + +// Function Definitions + +// +// Arguments : emxArray_char_T *emxArray +// int oldNumel +// Return Type : void +// +void emxEnsureCapacity_char_T(emxArray_char_T *emxArray, int oldNumel) +{ + int newNumel; + int i; + void *newData; + if (oldNumel < 0) { + oldNumel = 0; + } + + newNumel = 1; + for (i = 0; i < emxArray->numDimensions; i++) { + newNumel *= emxArray->size[i]; + } + + if (newNumel > emxArray->allocatedSize) { + i = emxArray->allocatedSize; + if (i < 16) { + i = 16; + } + + while (i < newNumel) { + if (i > 1073741823) { + i = MAX_int32_T; + } else { + i *= 2; + } + } + + newData = std::calloc(static_cast(i), sizeof(char)); + if (emxArray->data != NULL) { + std::memcpy(newData, emxArray->data, sizeof(char) * oldNumel); + if (emxArray->canFreeData) { + std::free(emxArray->data); + } + } + + emxArray->data = (char *)newData; + emxArray->allocatedSize = i; + emxArray->canFreeData = true; + } +} + +// +// Arguments : emxArray_creal_T *emxArray +// int oldNumel +// Return Type : void +// +void emxEnsureCapacity_creal_T(emxArray_creal_T *emxArray, int oldNumel) +{ + int newNumel; + int i; + void *newData; + if (oldNumel < 0) { + oldNumel = 0; + } + + newNumel = 1; + for (i = 0; i < emxArray->numDimensions; i++) { + newNumel *= emxArray->size[i]; + } + + if (newNumel > emxArray->allocatedSize) { + i = emxArray->allocatedSize; + if (i < 16) { + i = 16; + } + + while (i < newNumel) { + if (i > 1073741823) { + i = MAX_int32_T; + } else { + i *= 2; + } + } + + newData = std::calloc(static_cast(i), sizeof(creal_T)); + if (emxArray->data != NULL) { + std::memcpy(newData, emxArray->data, sizeof(creal_T) * oldNumel); + if (emxArray->canFreeData) { + std::free(emxArray->data); + } + } + + emxArray->data = (creal_T *)newData; + emxArray->allocatedSize = i; + emxArray->canFreeData = true; + } +} + +// +// Arguments : emxArray_real_T *emxArray +// int oldNumel +// Return Type : void +// +void emxEnsureCapacity_real_T(emxArray_real_T *emxArray, int oldNumel) +{ + int newNumel; + int i; + void *newData; + if (oldNumel < 0) { + oldNumel = 0; + } + + newNumel = 1; + for (i = 0; i < emxArray->numDimensions; i++) { + newNumel *= emxArray->size[i]; + } + + if (newNumel > emxArray->allocatedSize) { + i = emxArray->allocatedSize; + if (i < 16) { + i = 16; + } + + while (i < newNumel) { + if (i > 1073741823) { + i = MAX_int32_T; + } else { + i *= 2; + } + } + + newData = std::calloc(static_cast(i), sizeof(double)); + if (emxArray->data != NULL) { + std::memcpy(newData, emxArray->data, sizeof(double) * oldNumel); + if (emxArray->canFreeData) { + std::free(emxArray->data); + } + } + + emxArray->data = (double *)newData; + emxArray->allocatedSize = i; + emxArray->canFreeData = true; + } +} + +// +// Arguments : cwtfilterbank *pStruct +// Return Type : void +// +void emxFreeStruct_cwtfilterbank(cwtfilterbank *pStruct) +{ + emxFree_real_T(&pStruct->Scales); + emxFree_real_T(&pStruct->PsiDFT); + emxFree_real_T(&pStruct->WaveletCenterFrequencies); + emxFree_real_T(&pStruct->Omega); +} + +// +// Arguments : emxArray_char_T **pEmxArray +// Return Type : void +// +void emxFree_char_T(emxArray_char_T **pEmxArray) +{ + if (*pEmxArray != (emxArray_char_T *)NULL) { + if (((*pEmxArray)->data != (char *)NULL) && (*pEmxArray)->canFreeData) { + std::free((*pEmxArray)->data); + } + + std::free((*pEmxArray)->size); + std::free(*pEmxArray); + *pEmxArray = (emxArray_char_T *)NULL; + } +} + +// +// Arguments : emxArray_creal_T **pEmxArray +// Return Type : void +// +void emxFree_creal_T(emxArray_creal_T **pEmxArray) +{ + if (*pEmxArray != (emxArray_creal_T *)NULL) { + if (((*pEmxArray)->data != (creal_T *)NULL) && (*pEmxArray)->canFreeData) { + std::free((*pEmxArray)->data); + } + + std::free((*pEmxArray)->size); + std::free(*pEmxArray); + *pEmxArray = (emxArray_creal_T *)NULL; + } +} + +// +// Arguments : emxArray_real_T **pEmxArray +// Return Type : void +// +void emxFree_real_T(emxArray_real_T **pEmxArray) +{ + if (*pEmxArray != (emxArray_real_T *)NULL) { + if (((*pEmxArray)->data != (double *)NULL) && (*pEmxArray)->canFreeData) { + std::free((*pEmxArray)->data); + } + + std::free((*pEmxArray)->size); + std::free(*pEmxArray); + *pEmxArray = (emxArray_real_T *)NULL; + } +} + +// +// Arguments : cwtfilterbank *pStruct +// Return Type : void +// +void emxInitStruct_cwtfilterbank(cwtfilterbank *pStruct) +{ + emxInit_real_T(&pStruct->Scales, 2); + emxInit_real_T(&pStruct->PsiDFT, 2); + emxInit_real_T(&pStruct->WaveletCenterFrequencies, 1); + emxInit_real_T(&pStruct->Omega, 2); +} + +// +// Arguments : emxArray_char_T **pEmxArray +// int numDimensions +// Return Type : void +// +void emxInit_char_T(emxArray_char_T **pEmxArray, int numDimensions) +{ + emxArray_char_T *emxArray; + int i; + *pEmxArray = (emxArray_char_T *)std::malloc(sizeof(emxArray_char_T)); + emxArray = *pEmxArray; + emxArray->data = (char *)NULL; + emxArray->numDimensions = numDimensions; + emxArray->size = (int *)std::malloc(sizeof(int) * numDimensions); + emxArray->allocatedSize = 0; + emxArray->canFreeData = true; + for (i = 0; i < numDimensions; i++) { + emxArray->size[i] = 0; + } +} + +// +// Arguments : emxArray_creal_T **pEmxArray +// int numDimensions +// Return Type : void +// +void emxInit_creal_T(emxArray_creal_T **pEmxArray, int numDimensions) +{ + emxArray_creal_T *emxArray; + int i; + *pEmxArray = (emxArray_creal_T *)std::malloc(sizeof(emxArray_creal_T)); + emxArray = *pEmxArray; + emxArray->data = (creal_T *)NULL; + emxArray->numDimensions = numDimensions; + emxArray->size = (int *)std::malloc(sizeof(int) * numDimensions); + emxArray->allocatedSize = 0; + emxArray->canFreeData = true; + for (i = 0; i < numDimensions; i++) { + emxArray->size[i] = 0; + } +} + +// +// Arguments : emxArray_real_T **pEmxArray +// int numDimensions +// Return Type : void +// +void emxInit_real_T(emxArray_real_T **pEmxArray, int numDimensions) +{ + emxArray_real_T *emxArray; + int i; + *pEmxArray = (emxArray_real_T *)std::malloc(sizeof(emxArray_real_T)); + emxArray = *pEmxArray; + emxArray->data = (double *)NULL; + emxArray->numDimensions = numDimensions; + emxArray->size = (int *)std::malloc(sizeof(int) * numDimensions); + emxArray->allocatedSize = 0; + emxArray->canFreeData = true; + for (i = 0; i < numDimensions; i++) { + emxArray->size[i] = 0; + } +} + +// +// File trailer for cwt_emxutil.cpp +// +// [EOF] +// diff --git a/Classes/cwt_emxutil.h b/Classes/cwt_emxutil.h new file mode 100644 index 0000000..0e43d83 --- /dev/null +++ b/Classes/cwt_emxutil.h @@ -0,0 +1,36 @@ +// +// File: cwt_emxutil.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_EMXUTIL_H +#define CWT_EMXUTIL_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void emxEnsureCapacity_char_T(emxArray_char_T *emxArray, int oldNumel); +extern void emxEnsureCapacity_creal_T(emxArray_creal_T *emxArray, int oldNumel); +extern void emxEnsureCapacity_real_T(emxArray_real_T *emxArray, int oldNumel); +extern void emxFreeStruct_cwtfilterbank(cwtfilterbank *pStruct); +extern void emxFree_char_T(emxArray_char_T **pEmxArray); +extern void emxFree_creal_T(emxArray_creal_T **pEmxArray); +extern void emxFree_real_T(emxArray_real_T **pEmxArray); +extern void emxInitStruct_cwtfilterbank(cwtfilterbank *pStruct); +extern void emxInit_char_T(emxArray_char_T **pEmxArray, int numDimensions); +extern void emxInit_creal_T(emxArray_creal_T **pEmxArray, int numDimensions); +extern void emxInit_real_T(emxArray_real_T **pEmxArray, int numDimensions); + +#endif + +// +// File trailer for cwt_emxutil.h +// +// [EOF] +// diff --git a/Classes/cwt_initialize.cpp b/Classes/cwt_initialize.cpp new file mode 100644 index 0000000..e04b830 --- /dev/null +++ b/Classes/cwt_initialize.cpp @@ -0,0 +1,32 @@ +// +// File: cwt_initialize.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt_initialize.h" +#include "cwt.h" +#include "cwt_data.h" +#include "rt_nonfinite.h" +#include + +// Function Definitions + +// +// Arguments : void +// Return Type : void +// +void cwt_initialize() +{ + rt_InitInfAndNaN(); + omp_init_nest_lock(&emlrtNestLockGlobal); + isInitialized_cwt = true; +} + +// +// File trailer for cwt_initialize.cpp +// +// [EOF] +// diff --git a/Classes/cwt_initialize.h b/Classes/cwt_initialize.h new file mode 100644 index 0000000..1fd6532 --- /dev/null +++ b/Classes/cwt_initialize.h @@ -0,0 +1,26 @@ +// +// File: cwt_initialize.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_INITIALIZE_H +#define CWT_INITIALIZE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void cwt_initialize(); + +#endif + +// +// File trailer for cwt_initialize.h +// +// [EOF] +// diff --git a/Classes/cwt_rtwutil.cpp b/Classes/cwt_rtwutil.cpp new file mode 100644 index 0000000..d74859c --- /dev/null +++ b/Classes/cwt_rtwutil.cpp @@ -0,0 +1,73 @@ +// +// File: cwt_rtwutil.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt_rtwutil.h" +#include "cwt.h" +#include "rt_nonfinite.h" +#include +#include +#include + +// Function Definitions + +// +// Arguments : double u0 +// double u1 +// Return Type : double +// +double rt_powd_snf(double u0, double u1) +{ + double y; + double d; + double d1; + if (rtIsNaN(u0) || rtIsNaN(u1)) { + y = rtNaN; + } else { + d = std::abs(u0); + d1 = std::abs(u1); + if (rtIsInf(u1)) { + if (d == 1.0) { + y = 1.0; + } else if (d > 1.0) { + if (u1 > 0.0) { + y = rtInf; + } else { + y = 0.0; + } + } else if (u1 > 0.0) { + y = 0.0; + } else { + y = rtInf; + } + } else if (d1 == 0.0) { + y = 1.0; + } else if (d1 == 1.0) { + if (u1 > 0.0) { + y = u0; + } else { + y = 1.0 / u0; + } + } else if (u1 == 2.0) { + y = u0 * u0; + } else if ((u1 == 0.5) && (u0 >= 0.0)) { + y = std::sqrt(u0); + } else if ((u0 < 0.0) && (u1 > std::floor(u1))) { + y = rtNaN; + } else { + y = pow(u0, u1); + } + } + + return y; +} + +// +// File trailer for cwt_rtwutil.cpp +// +// [EOF] +// diff --git a/Classes/cwt_rtwutil.h b/Classes/cwt_rtwutil.h new file mode 100644 index 0000000..d4e5de9 --- /dev/null +++ b/Classes/cwt_rtwutil.h @@ -0,0 +1,26 @@ +// +// File: cwt_rtwutil.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_RTWUTIL_H +#define CWT_RTWUTIL_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern double rt_powd_snf(double u0, double u1); + +#endif + +// +// File trailer for cwt_rtwutil.h +// +// [EOF] +// diff --git a/Classes/cwt_terminate.cpp b/Classes/cwt_terminate.cpp new file mode 100644 index 0000000..385cf16 --- /dev/null +++ b/Classes/cwt_terminate.cpp @@ -0,0 +1,31 @@ +// +// File: cwt_terminate.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwt_terminate.h" +#include "cwt.h" +#include "cwt_data.h" +#include "rt_nonfinite.h" +#include + +// Function Definitions + +// +// Arguments : void +// Return Type : void +// +void cwt_terminate() +{ + omp_destroy_nest_lock(&emlrtNestLockGlobal); + isInitialized_cwt = false; +} + +// +// File trailer for cwt_terminate.cpp +// +// [EOF] +// diff --git a/Classes/cwt_terminate.h b/Classes/cwt_terminate.h new file mode 100644 index 0000000..5eec3f0 --- /dev/null +++ b/Classes/cwt_terminate.h @@ -0,0 +1,26 @@ +// +// File: cwt_terminate.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_TERMINATE_H +#define CWT_TERMINATE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void cwt_terminate(); + +#endif + +// +// File trailer for cwt_terminate.h +// +// [EOF] +// diff --git a/Classes/cwt_types.h b/Classes/cwt_types.h new file mode 100644 index 0000000..482402e --- /dev/null +++ b/Classes/cwt_types.h @@ -0,0 +1,67 @@ +// +// File: cwt_types.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWT_TYPES_H +#define CWT_TYPES_H + +// Include Files +#include "rtwtypes.h" + +// Type Definitions +struct emxArray_real_T +{ + double *data; + int *size; + int allocatedSize; + int numDimensions; + boolean_T canFreeData; +}; + +struct cwtfilterbank +{ + double VoicesPerOctave; + char Wavelet[5]; + double SamplingFrequency; + double SignalLength; + double FrequencyLimits[2]; + double TimeBandwidth; + double WaveletParameters[2]; + char Boundary[10]; + emxArray_real_T *Scales; + emxArray_real_T *PsiDFT; + emxArray_real_T *WaveletCenterFrequencies; + double Beta; + double Gamma; + double SignalPad; + emxArray_real_T *Omega; + double CutOff; +}; + +struct emxArray_creal_T +{ + creal_T *data; + int *size; + int allocatedSize; + int numDimensions; + boolean_T canFreeData; +}; + +struct emxArray_char_T +{ + char *data; + int *size; + int allocatedSize; + int numDimensions; + boolean_T canFreeData; +}; + +#endif + +// +// File trailer for cwt_types.h +// +// [EOF] +// diff --git a/Classes/cwtfilterbank.cpp b/Classes/cwtfilterbank.cpp new file mode 100644 index 0000000..631ab6e --- /dev/null +++ b/Classes/cwtfilterbank.cpp @@ -0,0 +1,997 @@ +// +// File: cwtfilterbank.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwtfilterbank.h" +#include "bsxfun.h" +#include "cwt.h" +#include "cwt_data.h" +#include "cwt_emxutil.h" +#include "cwt_rtwutil.h" +#include "cwtfreqlimits.h" +#include "fft.h" +#include "ifft.h" +#include "log2.h" +#include "rt_nonfinite.h" +#include "wavCFandSD.h" +#include +#include +#include +#include + +// Variable Definitions +static const char cv[10] = { 'r', 'e', 'f', 'l', 'e', 'c', 't', 'i', 'o', 'n' }; + +// Function Declarations +static void c_cwtfilterbank_validateInputsC(cwtfilterbank *self); +static void cwtfilterbank_filterbank(cwtfilterbank *self); +static int div_s32_floor(int numerator, int denominator); + +// Function Definitions + +// +// Arguments : cwtfilterbank *self +// Return Type : void +// +static void c_cwtfilterbank_validateInputsC(cwtfilterbank *self) +{ + boolean_T guard1 = false; + boolean_T b[2]; + boolean_T freqsep; + int ret; + boolean_T exitg1; + char a[10]; + int exitg2; + double x; + double freqrange_idx_0; + double freqrange_idx_1; + double fs; + double ga; + double be; + double omegac; + char y[5]; + char wav[5]; + double FourierFactor; + double cf; + double unusedU10; + static const char b_b[5] = { 'm', 'o', 'r', 's', 'e' }; + + emxArray_char_T *charStr; + int nbytes; + guard1 = false; + if (!rtIsNaN(self->TimeBandwidth)) { + b[0] = rtIsNaN(self->WaveletParameters[0]); + b[1] = rtIsNaN(self->WaveletParameters[1]); + freqsep = true; + ret = 0; + exitg1 = false; + while ((!exitg1) && (ret < 2)) { + if (!b[ret]) { + freqsep = false; + exitg1 = true; + } else { + ret++; + } + } + + if (freqsep) { + self->Beta = self->TimeBandwidth / self->Gamma; + } else { + guard1 = true; + } + } else { + guard1 = true; + } + + if (guard1) { + b[0] = rtIsNaN(self->WaveletParameters[0]); + b[1] = rtIsNaN(self->WaveletParameters[1]); + freqsep = true; + ret = 0; + exitg1 = false; + while ((!exitg1) && (ret < 2)) { + if (!b[ret]) { + freqsep = false; + exitg1 = true; + } else { + ret++; + } + } + + if ((!freqsep) && rtIsNaN(self->TimeBandwidth)) { + self->Gamma = self->WaveletParameters[0]; + self->Beta = self->WaveletParameters[1] / self->Gamma; + } + } + + for (ret = 0; ret < 10; ret++) { + a[ret] = self->Boundary[ret]; + } + + freqsep = false; + ret = 0; + do { + exitg2 = 0; + if (ret < 10) { + if (cv1[static_cast(a[ret]) & 127] != cv1[static_cast + (cv[ret])]) { + exitg2 = 1; + } else { + ret++; + } + } else { + freqsep = true; + exitg2 = 1; + } + } while (exitg2 == 0); + + if (freqsep) { + if (self->SignalLength <= 100000.0) { + self->SignalPad = std::floor(self->SignalLength / 2.0); + } else { + x = b_log2(self->SignalLength); + self->SignalPad = std::ceil(x); + } + } else { + self->SignalPad = 0.0; + } + + b[0] = rtIsNaN(self->FrequencyLimits[0]); + b[1] = rtIsNaN(self->FrequencyLimits[1]); + freqsep = true; + ret = 0; + exitg1 = false; + while ((!exitg1) && (ret < 2)) { + if (!b[ret]) { + freqsep = false; + exitg1 = true; + } else { + ret++; + } + } + + if (!freqsep) { + freqrange_idx_0 = self->FrequencyLimits[0]; + freqrange_idx_1 = self->FrequencyLimits[1]; + fs = self->SamplingFrequency; + ga = self->Gamma; + be = self->Beta; + for (ret = 0; ret < 5; ret++) { + y[ret] = self->Wavelet[ret]; + wav[ret] = cv1[static_cast(y[ret]) & 127]; + } + + omegac = 3.1415926535897931; + wavCFandSD(wav, ga, be, &FourierFactor, &x, &cf); + unusedU10 = self->SignalLength / (x * 2.0); + ret = memcmp(&wav[0], &b_b[0], 5); + if (ret == 0) { + ret = 0; + } else { + ret = -1; + } + + if (static_cast((ret == 0))) { + omegac = getFreqFromCutoffMorse(self->CutOff / 100.0, cf, ga, be); + } + + x = omegac / 3.1415926535897931 * rt_powd_snf(2.0, 1.0 / + self->VoicesPerOctave); + if (unusedU10 < x) { + unusedU10 = x; + } + + x = 1.0 / (unusedU10 * FourierFactor) * self->SamplingFrequency; + if (freqrange_idx_0 < x) { + self->FrequencyLimits[0] = x; + freqrange_idx_0 = self->FrequencyLimits[0]; + } + + if (freqrange_idx_1 > fs / 2.0) { + self->FrequencyLimits[1] = fs / 2.0; + freqrange_idx_1 = self->FrequencyLimits[1]; + } + + freqsep = (b_log2(freqrange_idx_1) - b_log2(freqrange_idx_0) >= 1.0 / + self->VoicesPerOctave); + if (!freqsep) { + emxInit_char_T(&charStr, 2); + x = 1.0 / self->VoicesPerOctave; + nbytes = (int)snprintf(NULL, 0, "%2.2f", x) + 1; + ret = charStr->size[0] * charStr->size[1]; + charStr->size[0] = 1; + charStr->size[1] = nbytes; + emxEnsureCapacity_char_T(charStr, ret); + snprintf(&charStr->data[0], (size_t)nbytes, "%2.2f", x); + emxFree_char_T(&charStr); + } + } +} + +// +// Arguments : cwtfilterbank *self +// Return Type : void +// +static void cwtfilterbank_filterbank(cwtfilterbank *self) +{ + int i; + boolean_T b_bool; + char a[5]; + int csz_idx_1; + int exitg1; + static const char b_cv[5] = { 'M', 'o', 'r', 's', 'e' }; + + emxArray_real_T *f; + emxArray_real_T *omega; + emxArray_real_T *absomega; + double ga; + double be; + emxArray_real_T *somega; + emxArray_real_T *c; + int bcoef; + int loop_ub; + double fo; + emxArray_real_T *powscales; + unsigned int unnamed_idx_0; + unsigned int unnamed_idx_1; + for (i = 0; i < 5; i++) { + a[i] = self->Wavelet[i]; + } + + b_bool = false; + csz_idx_1 = 0; + do { + exitg1 = 0; + if (csz_idx_1 < 5) { + if (cv1[static_cast(a[csz_idx_1]) & 127] != cv1[ + static_cast(b_cv[csz_idx_1])]) { + exitg1 = 1; + } else { + csz_idx_1++; + } + } else { + b_bool = true; + exitg1 = 1; + } + } while (exitg1 == 0); + + emxInit_real_T(&f, 2); + emxInit_real_T(&omega, 2); + emxInit_real_T(&absomega, 2); + if (b_bool) { + i = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = self->Omega->size[1]; + emxEnsureCapacity_real_T(omega, i); + csz_idx_1 = self->Omega->size[0] * self->Omega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + omega->data[i] = self->Omega->data[i]; + } + + i = f->size[0] * f->size[1]; + f->size[0] = 1; + f->size[1] = self->Scales->size[1]; + emxEnsureCapacity_real_T(f, i); + csz_idx_1 = self->Scales->size[0] * self->Scales->size[1]; + for (i = 0; i < csz_idx_1; i++) { + f->data[i] = self->Scales->data[i]; + } + + ga = self->Gamma; + be = self->Beta; + emxInit_real_T(&somega, 2); + if (f->size[1] == 1) { + emxInit_real_T(&c, 2); + if (omega->size[1] == 1) { + csz_idx_1 = 1; + } else { + csz_idx_1 = omega->size[1]; + } + + i = c->size[0] * c->size[1]; + c->size[0] = 1; + if (omega->size[1] == 1) { + c->size[1] = 1; + } else { + c->size[1] = omega->size[1]; + } + + emxEnsureCapacity_real_T(c, i); + if (csz_idx_1 != 0) { + bcoef = (omega->size[1] != 1); + i = csz_idx_1 - 1; + for (loop_ub = 0; loop_ub <= i; loop_ub++) { + c->data[loop_ub] = f->data[0] * omega->data[bcoef * loop_ub]; + } + } + + i = somega->size[0] * somega->size[1]; + somega->size[0] = 1; + somega->size[1] = c->size[1]; + emxEnsureCapacity_real_T(somega, i); + csz_idx_1 = c->size[0] * c->size[1]; + for (i = 0; i < csz_idx_1; i++) { + somega->data[i] = c->data[i]; + } + + emxFree_real_T(&c); + } else { + i = somega->size[0] * somega->size[1]; + somega->size[0] = f->size[1]; + somega->size[1] = omega->size[1]; + emxEnsureCapacity_real_T(somega, i); + csz_idx_1 = omega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + loop_ub = f->size[1]; + for (bcoef = 0; bcoef < loop_ub; bcoef++) { + somega->data[bcoef + somega->size[0] * i] = f->data[bcoef] * + omega->data[i]; + } + } + } + + fo = std::exp(1.0 / ga * (std::log(be) - std::log(ga))); + csz_idx_1 = somega->size[0] * somega->size[1]; + i = absomega->size[0] * absomega->size[1]; + absomega->size[0] = somega->size[0]; + absomega->size[1] = somega->size[1]; + emxEnsureCapacity_real_T(absomega, i); + for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) { + absomega->data[loop_ub] = std::abs(somega->data[loop_ub]); + } + + emxInit_real_T(&powscales, 2); + if (ga == 3.0) { + i = powscales->size[0] * powscales->size[1]; + powscales->size[0] = absomega->size[0]; + powscales->size[1] = absomega->size[1]; + emxEnsureCapacity_real_T(powscales, i); + csz_idx_1 = absomega->size[0] * absomega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + powscales->data[i] = absomega->data[i] * absomega->data[i] * + absomega->data[i]; + } + } else { + unnamed_idx_0 = static_cast(absomega->size[0]); + unnamed_idx_1 = static_cast(absomega->size[1]); + i = powscales->size[0] * powscales->size[1]; + powscales->size[0] = static_cast(unnamed_idx_0); + powscales->size[1] = static_cast(unnamed_idx_1); + emxEnsureCapacity_real_T(powscales, i); + csz_idx_1 = static_cast(unnamed_idx_0) * static_cast + (unnamed_idx_1); + for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) { + powscales->data[loop_ub] = rt_powd_snf(absomega->data[loop_ub], ga); + } + } + + csz_idx_1 = absomega->size[0] * absomega->size[1]; + for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) { + absomega->data[loop_ub] = std::log(absomega->data[loop_ub]); + } + + ga = 2.0 * std::exp(-be * std::log(fo) + rt_powd_snf(fo, ga)); + csz_idx_1 = absomega->size[0] * absomega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + absomega->data[i] = be * absomega->data[i] - powscales->data[i]; + } + + emxFree_real_T(&powscales); + csz_idx_1 = absomega->size[0] * absomega->size[1]; + for (loop_ub = 0; loop_ub < csz_idx_1; loop_ub++) { + absomega->data[loop_ub] = std::exp(absomega->data[loop_ub]); + } + + csz_idx_1 = absomega->size[0] * absomega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + absomega->data[i] = ga * absomega->data[i] * static_cast + ((somega->data[i] > 0.0)); + } + + emxFree_real_T(&somega); + i = f->size[0] * f->size[1]; + bcoef = f->size[0] * f->size[1]; + f->size[0] = 1; + emxEnsureCapacity_real_T(f, bcoef); + csz_idx_1 = i - 1; + for (i = 0; i <= csz_idx_1; i++) { + f->data[i] = fo / f->data[i] / 6.2831853071795862; + } + } else { + i = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = self->Omega->size[1]; + emxEnsureCapacity_real_T(omega, i); + csz_idx_1 = self->Omega->size[0] * self->Omega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + omega->data[i] = self->Omega->data[i]; + } + + i = f->size[0] * f->size[1]; + f->size[0] = 1; + f->size[1] = self->Scales->size[1]; + emxEnsureCapacity_real_T(f, i); + csz_idx_1 = self->Scales->size[0] * self->Scales->size[1]; + for (i = 0; i < csz_idx_1; i++) { + f->data[i] = self->Scales->data[i]; + } + + i = absomega->size[0] * absomega->size[1]; + absomega->size[0] = f->size[1]; + absomega->size[1] = omega->size[1]; + emxEnsureCapacity_real_T(absomega, i); + csz_idx_1 = f->size[1] * omega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + absomega->data[i] = 0.0; + } + + i = f->size[0] * f->size[1]; + bcoef = f->size[0] * f->size[1]; + f->size[0] = 1; + emxEnsureCapacity_real_T(f, bcoef); + csz_idx_1 = i - 1; + for (i = 0; i <= csz_idx_1; i++) { + f->data[i] = 0.0 / f->data[i]; + } + } + + emxFree_real_T(&omega); + i = f->size[0] * f->size[1]; + bcoef = f->size[0] * f->size[1]; + f->size[0] = 1; + emxEnsureCapacity_real_T(f, bcoef); + csz_idx_1 = i - 1; + for (i = 0; i <= csz_idx_1; i++) { + f->data[i] *= self->SamplingFrequency; + } + + i = self->PsiDFT->size[0] * self->PsiDFT->size[1]; + self->PsiDFT->size[0] = absomega->size[0]; + self->PsiDFT->size[1] = absomega->size[1]; + emxEnsureCapacity_real_T(self->PsiDFT, i); + csz_idx_1 = absomega->size[0] * absomega->size[1]; + for (i = 0; i < csz_idx_1; i++) { + self->PsiDFT->data[i] = absomega->data[i]; + } + + emxFree_real_T(&absomega); + i = self->WaveletCenterFrequencies->size[0]; + self->WaveletCenterFrequencies->size[0] = f->size[1]; + emxEnsureCapacity_real_T(self->WaveletCenterFrequencies, i); + csz_idx_1 = f->size[1]; + for (i = 0; i < csz_idx_1; i++) { + self->WaveletCenterFrequencies->data[i] = f->data[i]; + } + + emxFree_real_T(&f); +} + +// +// Arguments : int numerator +// int denominator +// Return Type : int +// +static int div_s32_floor(int numerator, int denominator) +{ + int quotient; + unsigned int absNumerator; + unsigned int absDenominator; + boolean_T quotientNeedsNegation; + unsigned int tempAbsQuotient; + if (denominator == 0) { + if (numerator >= 0) { + quotient = MAX_int32_T; + } else { + quotient = MIN_int32_T; + } + } else { + if (numerator < 0) { + absNumerator = ~static_cast(numerator) + 1U; + } else { + absNumerator = static_cast(numerator); + } + + if (denominator < 0) { + absDenominator = ~static_cast(denominator) + 1U; + } else { + absDenominator = static_cast(denominator); + } + + quotientNeedsNegation = ((numerator < 0) != (denominator < 0)); + tempAbsQuotient = absNumerator / absDenominator; + if (quotientNeedsNegation) { + absNumerator %= absDenominator; + if (absNumerator > 0U) { + tempAbsQuotient++; + } + + quotient = -static_cast(tempAbsQuotient); + } else { + quotient = static_cast(tempAbsQuotient); + } + } + + return quotient; +} + +// +// Arguments : cwtfilterbank *self +// double varargin_2 +// Return Type : cwtfilterbank * +// +cwtfilterbank *cwtfilterbank_cwtfilterbank(cwtfilterbank *self, double + varargin_2) +{ + cwtfilterbank *b_self; + int nx; + static const char b_cv[5] = { 'M', 'o', 'r', 's', 'e' }; + + double frange_idx_1; + boolean_T b_bool; + char a[5]; + int ret; + int exitg1; + static const char b_cv1[5] = { 'M', 'o', 'r', 's', 'e' }; + + double N; + emxArray_real_T *omega; + int loop_ub; + int i; + int i1; + emxArray_real_T *y; + double frange_idx_0; + boolean_T b[2]; + boolean_T exitg2; + char x[5]; + double be; + double nv; + double cutoff; + double FourierFactor; + double omegac; + double cf; + static const char b_b[5] = { 'm', 'o', 'r', 's', 'e' }; + + b_self = self; + b_self->CutOff = 50.0; + b_self->Gamma = 3.0; + b_self->Beta = 20.0; + for (nx = 0; nx < 5; nx++) { + b_self->Wavelet[nx] = b_cv[nx]; + } + + b_self->TimeBandwidth = rtNaN; + b_self->WaveletParameters[0] = rtNaN; + b_self->WaveletParameters[1] = rtNaN; + b_self->SignalLength = varargin_2; + frange_idx_1 = b_self->SignalLength / 2.0; + frange_idx_1 = std::floor(frange_idx_1); + b_self->SignalPad = frange_idx_1; + b_self->VoicesPerOctave = 10.0; + b_self->SamplingFrequency = 1.0; + b_self->FrequencyLimits[0] = rtNaN; + b_self->FrequencyLimits[1] = rtNaN; + for (nx = 0; nx < 10; nx++) { + b_self->Boundary[nx] = cv[nx]; + } + + c_cwtfilterbank_validateInputsC(b_self); + for (nx = 0; nx < 5; nx++) { + a[nx] = b_self->Wavelet[nx]; + } + + b_bool = false; + ret = 0; + do { + exitg1 = 0; + if (ret < 5) { + if (cv1[static_cast(a[ret]) & 127] != cv1[static_cast + (b_cv1[ret])]) { + exitg1 = 1; + } else { + ret++; + } + } else { + b_bool = true; + exitg1 = 1; + } + } while (exitg1 == 0); + + if (!b_bool) { + b_self->CutOff = 10.0; + } + + N = b_self->SignalLength + 2.0 * b_self->SignalPad; + frange_idx_1 = N / 2.0; + if (frange_idx_1 < 0.0) { + frange_idx_1 = std::ceil(frange_idx_1); + } else { + frange_idx_1 = std::floor(frange_idx_1); + } + + emxInit_real_T(&omega, 2); + if (rtIsNaN(frange_idx_1)) { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = 1; + emxEnsureCapacity_real_T(omega, nx); + omega->data[0] = rtNaN; + } else if (frange_idx_1 < 1.0) { + omega->size[0] = 1; + omega->size[1] = 0; + } else if (rtIsInf(frange_idx_1) && (1.0 == frange_idx_1)) { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = 1; + emxEnsureCapacity_real_T(omega, nx); + omega->data[0] = rtNaN; + } else { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = static_cast((frange_idx_1 - 1.0)) + 1; + emxEnsureCapacity_real_T(omega, nx); + loop_ub = static_cast((frange_idx_1 - 1.0)); + for (nx = 0; nx <= loop_ub; nx++) { + omega->data[nx] = static_cast(nx) + 1.0; + } + } + + nx = omega->size[0] * omega->size[1]; + i = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + emxEnsureCapacity_real_T(omega, i); + loop_ub = nx - 1; + for (nx = 0; nx <= loop_ub; nx++) { + omega->data[nx] = omega->data[nx] * 6.2831853071795862 / N; + } + + frange_idx_1 = (N - 1.0) / 2.0; + if (frange_idx_1 < 0.0) { + frange_idx_1 = std::ceil(frange_idx_1); + } else { + frange_idx_1 = std::floor(frange_idx_1); + } + + if (1.0 > frange_idx_1) { + nx = 0; + i = 1; + i1 = -1; + } else { + nx = static_cast(frange_idx_1) - 1; + i = -1; + i1 = 0; + } + + emxInit_real_T(&y, 2); + ret = y->size[0] * y->size[1]; + y->size[0] = 1; + loop_ub = div_s32_floor(i1 - nx, i); + y->size[1] = (omega->size[1] + loop_ub) + 2; + emxEnsureCapacity_real_T(y, ret); + y->data[0] = 0.0; + ret = omega->size[1]; + for (i1 = 0; i1 < ret; i1++) { + y->data[i1 + 1] = omega->data[i1]; + } + + for (i1 = 0; i1 <= loop_ub; i1++) { + y->data[(i1 + omega->size[1]) + 1] = -omega->data[nx + i * i1]; + } + + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = y->size[1]; + emxEnsureCapacity_real_T(omega, nx); + loop_ub = y->size[0] * y->size[1]; + for (nx = 0; nx < loop_ub; nx++) { + omega->data[nx] = y->data[nx]; + } + + nx = b_self->Omega->size[0] * b_self->Omega->size[1]; + b_self->Omega->size[0] = 1; + b_self->Omega->size[1] = omega->size[1]; + emxEnsureCapacity_real_T(b_self->Omega, nx); + loop_ub = omega->size[0] * omega->size[1]; + for (nx = 0; nx < loop_ub; nx++) { + b_self->Omega->data[nx] = omega->data[nx]; + } + + frange_idx_0 = b_self->FrequencyLimits[0]; + frange_idx_1 = b_self->FrequencyLimits[1]; + b[0] = rtIsNaN(frange_idx_0); + b[1] = rtIsNaN(frange_idx_1); + b_bool = true; + ret = 0; + exitg2 = false; + while ((!exitg2) && (ret < 2)) { + if (!b[ret]) { + b_bool = false; + exitg2 = true; + } else { + ret++; + } + } + + if (!b_bool) { + frange_idx_0 = b_self->FrequencyLimits[0]; + frange_idx_1 = b_self->FrequencyLimits[1]; + frange_idx_0 /= b_self->SamplingFrequency; + frange_idx_1 /= b_self->SamplingFrequency; + frange_idx_0 = frange_idx_0 * 2.0 * 3.1415926535897931; + frange_idx_1 = frange_idx_1 * 2.0 * 3.1415926535897931; + nv = b_self->VoicesPerOctave; + wavCFandSD(b_self->Wavelet, b_self->Gamma, b_self->Beta, &FourierFactor, + &cutoff, &be); + omegac = be / frange_idx_1; + frange_idx_1 = b_log2(be / frange_idx_0 / omegac); + frange_idx_1 *= nv; + if (rtIsNaN(frange_idx_1)) { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = 1; + emxEnsureCapacity_real_T(omega, nx); + omega->data[0] = rtNaN; + } else if (frange_idx_1 < 0.0) { + omega->size[0] = 1; + omega->size[1] = 0; + } else if (rtIsInf(frange_idx_1) && (0.0 == frange_idx_1)) { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = 1; + emxEnsureCapacity_real_T(omega, nx); + omega->data[0] = rtNaN; + } else { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + loop_ub = static_cast(std::floor(frange_idx_1)); + omega->size[1] = loop_ub + 1; + emxEnsureCapacity_real_T(omega, nx); + for (nx = 0; nx <= loop_ub; nx++) { + omega->data[nx] = nx; + } + } + + cutoff = rt_powd_snf(2.0, 1.0 / nv); + nx = y->size[0] * y->size[1]; + y->size[0] = 1; + y->size[1] = omega->size[1]; + emxEnsureCapacity_real_T(y, nx); + nx = omega->size[1]; + for (ret = 0; ret < nx; ret++) { + y->data[ret] = rt_powd_snf(cutoff, omega->data[ret]); + } + + nx = b_self->Scales->size[0] * b_self->Scales->size[1]; + b_self->Scales->size[0] = 1; + b_self->Scales->size[1] = y->size[1]; + emxEnsureCapacity_real_T(b_self->Scales, nx); + loop_ub = y->size[0] * y->size[1]; + for (nx = 0; nx < loop_ub; nx++) { + b_self->Scales->data[nx] = omegac * y->data[nx]; + } + } else { + for (nx = 0; nx < 5; nx++) { + x[nx] = b_self->Wavelet[nx]; + } + + N = b_self->SignalLength; + frange_idx_0 = b_self->Gamma; + be = b_self->Beta; + nv = b_self->VoicesPerOctave; + cutoff = b_self->CutOff; + for (ret = 0; ret < 5; ret++) { + a[ret] = cv1[static_cast(x[ret]) & 127]; + } + + omegac = 3.1415926535897931; + cutoff /= 100.0; + wavCFandSD(a, frange_idx_0, be, &FourierFactor, &frange_idx_1, &cf); + FourierFactor = N / (frange_idx_1 * 2.0); + ret = memcmp(&a[0], &b_b[0], 5); + if (ret == 0) { + ret = 0; + } else { + ret = -1; + } + + if (static_cast((ret == 0))) { + omegac = getFreqFromCutoffMorse(cutoff, cf, frange_idx_0, be); + } + + omegac /= 3.1415926535897931; + cutoff = rt_powd_snf(2.0, 1.0 / nv); + frange_idx_1 = omegac * cutoff; + if (FourierFactor < frange_idx_1) { + FourierFactor = frange_idx_1; + } + + frange_idx_1 = b_log2(FourierFactor / omegac); + FourierFactor = 1.0 / nv; + if ((frange_idx_1 > FourierFactor) || rtIsNaN(FourierFactor)) { + FourierFactor = frange_idx_1; + } + + frange_idx_1 = FourierFactor * nv; + if (rtIsNaN(frange_idx_1)) { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = 1; + emxEnsureCapacity_real_T(omega, nx); + omega->data[0] = rtNaN; + } else if (frange_idx_1 < 0.0) { + omega->size[0] = 1; + omega->size[1] = 0; + } else if (rtIsInf(frange_idx_1) && (0.0 == frange_idx_1)) { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + omega->size[1] = 1; + emxEnsureCapacity_real_T(omega, nx); + omega->data[0] = rtNaN; + } else { + nx = omega->size[0] * omega->size[1]; + omega->size[0] = 1; + loop_ub = static_cast(std::floor(frange_idx_1)); + omega->size[1] = loop_ub + 1; + emxEnsureCapacity_real_T(omega, nx); + for (nx = 0; nx <= loop_ub; nx++) { + omega->data[nx] = nx; + } + } + + nx = y->size[0] * y->size[1]; + y->size[0] = 1; + y->size[1] = omega->size[1]; + emxEnsureCapacity_real_T(y, nx); + nx = omega->size[1]; + for (ret = 0; ret < nx; ret++) { + y->data[ret] = rt_powd_snf(cutoff, omega->data[ret]); + } + + nx = b_self->Scales->size[0] * b_self->Scales->size[1]; + b_self->Scales->size[0] = 1; + b_self->Scales->size[1] = y->size[1]; + emxEnsureCapacity_real_T(b_self->Scales, nx); + loop_ub = y->size[0] * y->size[1]; + for (nx = 0; nx < loop_ub; nx++) { + b_self->Scales->data[nx] = omegac * y->data[nx]; + } + } + + emxFree_real_T(&y); + emxFree_real_T(&omega); + cwtfilterbank_filterbank(b_self); + return b_self; +} + +// +// Arguments : const cwtfilterbank *self +// double x_data[] +// const int x_size[2] +// emxArray_creal_T *varargout_1 +// Return Type : void +// +void cwtfilterbank_wt(const cwtfilterbank *self, double x_data[], const int + x_size[2], emxArray_creal_T *varargout_1) +{ + int nd2; + int loop_ub; + double b_x_data[1280]; + int xv_size[2]; + double xv_data[3841]; + double xtmp; + emxArray_creal_T *cfspos; + emxArray_creal_T *r; + static creal_T tmp_data[3841]; + int tmp_size[2]; + int i; + int i1; + int i2; + int b_j1; + int j2; + nd2 = x_size[1]; + loop_ub = x_size[1]; + if (0 <= loop_ub - 1) { + std::memcpy(&b_x_data[0], &x_data[0], loop_ub * sizeof(double)); + } + + if (0 <= nd2 - 1) { + std::memcpy(&x_data[0], &b_x_data[0], nd2 * sizeof(double)); + } + + xv_size[0] = 1; + xv_size[1] = nd2; + if (0 <= nd2 - 1) { + std::memcpy(&xv_data[0], &x_data[0], nd2 * sizeof(double)); + } + + if (self->SignalPad > 0.0) { + xtmp = self->SignalPad; + if (1.0 > xtmp) { + loop_ub = 0; + } else { + loop_ub = static_cast(xtmp); + } + + xtmp = (static_cast(nd2) - self->SignalPad) + 1.0; + if (xtmp > nd2) { + i = 0; + i1 = 1; + i2 = -1; + } else { + i = nd2 - 1; + i1 = -1; + i2 = static_cast(xtmp) - 1; + } + + if (0 <= loop_ub - 1) { + std::memcpy(&b_x_data[0], &x_data[0], loop_ub * sizeof(double)); + } + + nd2 = loop_ub >> 1; + for (b_j1 = 0; b_j1 < nd2; b_j1++) { + j2 = (static_cast(loop_ub) - b_j1) - 1; + xtmp = b_x_data[b_j1]; + b_x_data[b_j1] = b_x_data[j2]; + b_x_data[j2] = xtmp; + } + + xv_size[0] = 1; + nd2 = div_s32_floor(i2 - i, i1); + xv_size[1] = ((loop_ub + x_size[1]) + nd2) + 1; + if (0 <= loop_ub - 1) { + std::memcpy(&xv_data[0], &b_x_data[0], loop_ub * sizeof(double)); + } + + b_j1 = x_size[1]; + for (i2 = 0; i2 < b_j1; i2++) { + xv_data[i2 + loop_ub] = x_data[i2]; + } + + for (i2 = 0; i2 <= nd2; i2++) { + xv_data[(i2 + loop_ub) + x_size[1]] = x_data[i + i1 * i2]; + } + } + + emxInit_creal_T(&cfspos, 2); + emxInit_creal_T(&r, 2); + fft(xv_data, xv_size, tmp_data, tmp_size); + bsxfun(tmp_data, tmp_size, self->PsiDFT, r); + ifft(r, cfspos); + i = varargout_1->size[0] * varargout_1->size[1]; + varargout_1->size[0] = cfspos->size[0]; + varargout_1->size[1] = cfspos->size[1]; + emxEnsureCapacity_creal_T(varargout_1, i); + loop_ub = cfspos->size[0] * cfspos->size[1]; + emxFree_creal_T(&r); + for (i = 0; i < loop_ub; i++) { + varargout_1->data[i] = cfspos->data[i]; + } + + if (self->SignalPad > 0.0) { + xtmp = self->SignalPad; + loop_ub = cfspos->size[0]; + i = varargout_1->size[0] * varargout_1->size[1]; + varargout_1->size[0] = cfspos->size[0]; + nd2 = static_cast(std::floor(self->SignalLength - 1.0)); + varargout_1->size[1] = nd2 + 1; + emxEnsureCapacity_creal_T(varargout_1, i); + for (i = 0; i <= nd2; i++) { + for (i1 = 0; i1 < loop_ub; i1++) { + varargout_1->data[i1 + varargout_1->size[0] * i] = cfspos->data[i1 + + cfspos->size[0] * (static_cast((xtmp + static_cast((i + 1)))) + - 1)]; + } + } + } + + emxFree_creal_T(&cfspos); +} + +// +// File trailer for cwtfilterbank.cpp +// +// [EOF] +// diff --git a/Classes/cwtfilterbank.h b/Classes/cwtfilterbank.h new file mode 100644 index 0000000..d5c5a19 --- /dev/null +++ b/Classes/cwtfilterbank.h @@ -0,0 +1,29 @@ +// +// File: cwtfilterbank.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWTFILTERBANK_H +#define CWTFILTERBANK_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern cwtfilterbank *cwtfilterbank_cwtfilterbank(cwtfilterbank *self, double + varargin_2); +extern void cwtfilterbank_wt(const cwtfilterbank *self, double x_data[], const + int x_size[2], emxArray_creal_T *varargout_1); + +#endif + +// +// File trailer for cwtfilterbank.h +// +// [EOF] +// diff --git a/Classes/cwtfreqlimits.cpp b/Classes/cwtfreqlimits.cpp new file mode 100644 index 0000000..f63c785 --- /dev/null +++ b/Classes/cwtfreqlimits.cpp @@ -0,0 +1,149 @@ +// +// File: cwtfreqlimits.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "cwtfreqlimits.h" +#include "cwt.h" +#include "cwt_rtwutil.h" +#include "cwtfilterbank.h" +#include "rt_nonfinite.h" +#include +#include + +// Function Definitions + +// +// Arguments : double cutoff +// double cf +// double ga +// double be +// Return Type : double +// +double getFreqFromCutoffMorse(double cutoff, double cf, double ga, double be) +{ + double omegac; + double psihat_tunableEnvironment_idx_1; + double psihat_tunableEnvironment_idx_0; + double fa; + double a; + double fb; + double fc; + double c; + double e; + double d; + boolean_T exitg1; + double m; + double p; + double toler; + double s; + double r; + psihat_tunableEnvironment_idx_1 = 2.0 * std::exp(be / ga * ((std::log(ga) - + std::log(be)) + 1.0)); + psihat_tunableEnvironment_idx_0 = 2.0 * cutoff; + omegac = rt_powd_snf(750.0, 1.0 / ga); + fa = psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 * + rt_powd_snf(cf, be) * std::exp(-rt_powd_snf(cf, ga)); + if (fa >= 0.0) { + if (!(psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 * + rt_powd_snf(omegac, be) * std::exp(-rt_powd_snf(omegac, ga)) == + psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 * + rt_powd_snf(cf, be) * std::exp(-rt_powd_snf(cf, ga)))) { + omegac = cf; + } + } else { + a = cf; + fb = psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 * + rt_powd_snf(omegac, be) * std::exp(-rt_powd_snf(omegac, ga)); + if (!(fb == 0.0)) { + fc = fb; + c = omegac; + e = 0.0; + d = 0.0; + exitg1 = false; + while ((!exitg1) && ((fb != 0.0) && (a != omegac))) { + if ((fb > 0.0) == (fc > 0.0)) { + c = a; + fc = fa; + d = omegac - a; + e = d; + } + + if (std::abs(fc) < std::abs(fb)) { + a = omegac; + omegac = c; + c = a; + fa = fb; + fb = fc; + fc = fa; + } + + m = 0.5 * (c - omegac); + p = std::abs(omegac); + if (!(p > 1.0)) { + p = 1.0; + } + + toler = 4.4408920985006262E-16 * p; + if ((std::abs(m) <= toler) || (fb == 0.0)) { + exitg1 = true; + } else { + if ((std::abs(e) < toler) || (std::abs(fa) <= std::abs(fb))) { + d = m; + e = m; + } else { + s = fb / fa; + if (a == c) { + p = 2.0 * m * s; + fa = 1.0 - s; + } else { + fa /= fc; + r = fb / fc; + p = s * (2.0 * m * fa * (fa - r) - (omegac - a) * (r - 1.0)); + fa = (fa - 1.0) * (r - 1.0) * (s - 1.0); + } + + if (p > 0.0) { + fa = -fa; + } else { + p = -p; + } + + if ((2.0 * p < 3.0 * m * fa - std::abs(toler * fa)) && (p < std::abs + (0.5 * e * fa))) { + e = d; + d = p / fa; + } else { + d = m; + e = m; + } + } + + a = omegac; + fa = fb; + if (std::abs(d) > toler) { + omegac += d; + } else if (omegac > c) { + omegac -= toler; + } else { + omegac += toler; + } + + fb = psihat_tunableEnvironment_idx_0 - psihat_tunableEnvironment_idx_1 + * rt_powd_snf(omegac, be) * std::exp(-rt_powd_snf(omegac, ga)); + } + } + } + } + + return omegac; +} + +// +// File trailer for cwtfreqlimits.cpp +// +// [EOF] +// diff --git a/Classes/cwtfreqlimits.h b/Classes/cwtfreqlimits.h new file mode 100644 index 0000000..0a179e7 --- /dev/null +++ b/Classes/cwtfreqlimits.h @@ -0,0 +1,27 @@ +// +// File: cwtfreqlimits.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef CWTFREQLIMITS_H +#define CWTFREQLIMITS_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern double getFreqFromCutoffMorse(double cutoff, double cf, double ga, double + be); + +#endif + +// +// File trailer for cwtfreqlimits.h +// +// [EOF] +// diff --git a/Classes/fft.cpp b/Classes/fft.cpp new file mode 100644 index 0000000..311b105 --- /dev/null +++ b/Classes/fft.cpp @@ -0,0 +1,69 @@ +// +// File: fft.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "fft.h" +#include "cwt.h" +#include "fft1.h" +#include "rt_nonfinite.h" +#include +#include + +// Function Definitions + +// +// Arguments : const double x_data[] +// const int x_size[2] +// creal_T y_data[] +// int y_size[2] +// Return Type : void +// +void fft(const double x_data[], const int x_size[2], creal_T y_data[], int + y_size[2]) +{ + boolean_T useRadix2; + int N2blue; + int nRows; + static double costab_data[7683]; + int costab_size[2]; + static double sintab_data[7683]; + int sintab_size[2]; + double sintabinv_data[7683]; + int sintabinv_size[2]; + int x[1]; + creal_T b_y_data[3841]; + int b_y_size[1]; + if (x_size[1] != 0) { + useRadix2 = ((x_size[1] & (x_size[1] - 1)) == 0); + get_algo_sizes(x_size[1], useRadix2, &N2blue, &nRows); + generate_twiddle_tables(nRows, useRadix2, costab_data, costab_size, + sintab_data, sintab_size, sintabinv_data, sintabinv_size); + if (useRadix2) { + x[0] = x_size[1]; + r2br_r2dit_trig(x_data, x, x_size[1], costab_data, sintab_data, b_y_data, + b_y_size); + } else { + x[0] = x_size[1]; + dobluesteinfft(x_data, x, N2blue, x_size[1], costab_data, costab_size, + sintab_data, sintab_size, sintabinv_data, sintabinv_size, + b_y_data, b_y_size); + } + } + + y_size[0] = 1; + y_size[1] = x_size[1]; + N2blue = x_size[1]; + if (0 <= N2blue - 1) { + std::memcpy(&y_data[0], &b_y_data[0], N2blue * sizeof(creal_T)); + } +} + +// +// File trailer for fft.cpp +// +// [EOF] +// diff --git a/Classes/fft.h b/Classes/fft.h new file mode 100644 index 0000000..140cbb6 --- /dev/null +++ b/Classes/fft.h @@ -0,0 +1,27 @@ +// +// File: fft.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef FFT_H +#define FFT_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void fft(const double x_data[], const int x_size[2], creal_T y_data[], + int y_size[2]); + +#endif + +// +// File trailer for fft.h +// +// [EOF] +// diff --git a/Classes/fft1.cpp b/Classes/fft1.cpp new file mode 100644 index 0000000..78eef99 --- /dev/null +++ b/Classes/fft1.cpp @@ -0,0 +1,1073 @@ +// +// File: fft1.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "fft1.h" +#include "cwt.h" +#include "cwt_emxutil.h" +#include "rt_nonfinite.h" +#include +#include +#include + +// Function Declarations +static void bluestein(const double x_data[], const int x_size[1], int nfft, int + nRows, const double costab_data[], const int costab_size[2], + const double sintab_data[], const int sintab_size[2], + const double costabinv_data[], const int costabinv_size[2], + const double sintabinv_data[], const int sintabinv_size[2], + const emxArray_creal_T *wwc, creal_T y_data[], int y_size + [1]); +static void r2br_r2dit_trig_impl(const emxArray_creal_T *x, int unsigned_nRows, + const emxArray_real_T *costab, const emxArray_real_T *sintab, emxArray_creal_T + *y); + +// Function Definitions + +// +// Arguments : const double x_data[] +// const int x_size[1] +// int nfft +// int nRows +// const double costab_data[] +// const int costab_size[2] +// const double sintab_data[] +// const int sintab_size[2] +// const double costabinv_data[] +// const int costabinv_size[2] +// const double sintabinv_data[] +// const int sintabinv_size[2] +// const emxArray_creal_T *wwc +// creal_T y_data[] +// int y_size[1] +// Return Type : void +// +static void bluestein(const double x_data[], const int x_size[1], int nfft, int + nRows, const double costab_data[], const int costab_size[2], + const double sintab_data[], const int sintab_size[2], + const double costabinv_data[], const int costabinv_size[2], + const double sintabinv_data[], const int sintabinv_size[2], + const emxArray_creal_T *wwc, creal_T y_data[], int y_size + [1]) +{ + int minNrowsNx; + int xidx; + int k; + int a_re_tmp; + emxArray_creal_T *y; + emxArray_creal_T *r; + emxArray_creal_T *b_y; + emxArray_creal_T b_y_data; + emxArray_real_T b_costab_data; + emxArray_real_T b_sintab_data; + emxArray_real_T c_costab_data; + emxArray_real_T c_sintab_data; + emxArray_real_T b_costabinv_data; + emxArray_real_T b_sintabinv_data; + double b; + minNrowsNx = x_size[0]; + if (nRows < minNrowsNx) { + minNrowsNx = nRows; + } + + y_size[0] = nRows; + if (nRows > x_size[0]) { + y_size[0] = nRows; + if (0 <= nRows - 1) { + std::memset(&y_data[0], 0, nRows * sizeof(creal_T)); + } + } + + xidx = 0; + for (k = 0; k < minNrowsNx; k++) { + a_re_tmp = (nRows + k) - 1; + y_data[k].re = wwc->data[a_re_tmp].re * x_data[xidx]; + y_data[k].im = wwc->data[a_re_tmp].im * -x_data[xidx]; + xidx++; + } + + xidx = minNrowsNx + 1; + if (xidx <= nRows) { + std::memset(&y_data[xidx + -1], 0, ((nRows - xidx) + 1) * sizeof(creal_T)); + } + + emxInit_creal_T(&y, 1); + emxInit_creal_T(&r, 1); + emxInit_creal_T(&b_y, 1); + b_y_data.data = &y_data[0]; + b_y_data.size = &y_size[0]; + b_y_data.allocatedSize = -1; + b_y_data.numDimensions = 1; + b_y_data.canFreeData = false; + b_costab_data.data = const_cast(&costab_data[0]); + b_costab_data.size = const_cast(&costab_size[0]); + b_costab_data.allocatedSize = -1; + b_costab_data.numDimensions = 2; + b_costab_data.canFreeData = false; + b_sintab_data.data = const_cast(&sintab_data[0]); + b_sintab_data.size = const_cast(&sintab_size[0]); + b_sintab_data.allocatedSize = -1; + b_sintab_data.numDimensions = 2; + b_sintab_data.canFreeData = false; + r2br_r2dit_trig_impl(&b_y_data, nfft, &b_costab_data, &b_sintab_data, y); + c_costab_data.data = const_cast(&costab_data[0]); + c_costab_data.size = const_cast(&costab_size[0]); + c_costab_data.allocatedSize = -1; + c_costab_data.numDimensions = 2; + c_costab_data.canFreeData = false; + c_sintab_data.data = const_cast(&sintab_data[0]); + c_sintab_data.size = const_cast(&sintab_size[0]); + c_sintab_data.allocatedSize = -1; + c_sintab_data.numDimensions = 2; + c_sintab_data.canFreeData = false; + r2br_r2dit_trig_impl(wwc, nfft, &c_costab_data, &c_sintab_data, r); + xidx = b_y->size[0]; + b_y->size[0] = y->size[0]; + emxEnsureCapacity_creal_T(b_y, xidx); + minNrowsNx = y->size[0]; + for (xidx = 0; xidx < minNrowsNx; xidx++) { + b_y->data[xidx].re = y->data[xidx].re * r->data[xidx].re - y->data[xidx].im * + r->data[xidx].im; + b_y->data[xidx].im = y->data[xidx].re * r->data[xidx].im + y->data[xidx].im * + r->data[xidx].re; + } + + emxFree_creal_T(&r); + b_costabinv_data.data = const_cast(&costabinv_data[0]); + b_costabinv_data.size = const_cast(&costabinv_size[0]); + b_costabinv_data.allocatedSize = -1; + b_costabinv_data.numDimensions = 2; + b_costabinv_data.canFreeData = false; + b_sintabinv_data.data = const_cast(&sintabinv_data[0]); + b_sintabinv_data.size = const_cast(&sintabinv_size[0]); + b_sintabinv_data.allocatedSize = -1; + b_sintabinv_data.numDimensions = 2; + b_sintabinv_data.canFreeData = false; + r2br_r2dit_trig_impl(b_y, nfft, &b_costabinv_data, &b_sintabinv_data, y); + emxFree_creal_T(&b_y); + if (y->size[0] > 1) { + b = 1.0 / static_cast(y->size[0]); + minNrowsNx = y->size[0]; + for (xidx = 0; xidx < minNrowsNx; xidx++) { + y->data[xidx].re *= b; + y->data[xidx].im *= b; + } + } + + minNrowsNx = 0; + xidx = wwc->size[0]; + for (k = nRows; k <= xidx; k++) { + y_data[minNrowsNx].re = wwc->data[k - 1].re * y->data[k - 1].re + wwc-> + data[k - 1].im * y->data[k - 1].im; + y_data[minNrowsNx].im = wwc->data[k - 1].re * y->data[k - 1].im - wwc-> + data[k - 1].im * y->data[k - 1].re; + minNrowsNx++; + } + + emxFree_creal_T(&y); +} + +// +// Arguments : const emxArray_creal_T *x +// int unsigned_nRows +// const emxArray_real_T *costab +// const emxArray_real_T *sintab +// emxArray_creal_T *y +// Return Type : void +// +static void r2br_r2dit_trig_impl(const emxArray_creal_T *x, int unsigned_nRows, + const emxArray_real_T *costab, const emxArray_real_T *sintab, emxArray_creal_T + *y) +{ + int istart; + int nRowsM2; + int nRowsD2; + int nRowsD4; + int iy; + int ix; + int ju; + int i; + boolean_T tst; + double temp_re; + double temp_im; + double twid_re; + double twid_im; + int temp_re_tmp; + int ihi; + istart = x->size[0]; + if (istart >= unsigned_nRows) { + istart = unsigned_nRows; + } + + nRowsM2 = unsigned_nRows - 2; + nRowsD2 = unsigned_nRows / 2; + nRowsD4 = nRowsD2 / 2; + iy = y->size[0]; + y->size[0] = unsigned_nRows; + emxEnsureCapacity_creal_T(y, iy); + if (unsigned_nRows > x->size[0]) { + iy = y->size[0]; + y->size[0] = unsigned_nRows; + emxEnsureCapacity_creal_T(y, iy); + for (iy = 0; iy < unsigned_nRows; iy++) { + y->data[iy].re = 0.0; + y->data[iy].im = 0.0; + } + } + + ix = 0; + ju = 0; + iy = 0; + for (i = 0; i <= istart - 2; i++) { + y->data[iy] = x->data[ix]; + iy = unsigned_nRows; + tst = true; + while (tst) { + iy >>= 1; + ju ^= iy; + tst = ((ju & iy) == 0); + } + + iy = ju; + ix++; + } + + y->data[iy] = x->data[ix]; + if (unsigned_nRows > 1) { + for (i = 0; i <= nRowsM2; i += 2) { + temp_re = y->data[i + 1].re; + temp_im = y->data[i + 1].im; + twid_re = y->data[i].re; + twid_im = y->data[i].im; + y->data[i + 1].re = y->data[i].re - y->data[i + 1].re; + y->data[i + 1].im = y->data[i].im - y->data[i + 1].im; + twid_re += temp_re; + twid_im += temp_im; + y->data[i].re = twid_re; + y->data[i].im = twid_im; + } + } + + iy = 2; + ix = 4; + ju = ((nRowsD4 - 1) << 2) + 1; + while (nRowsD4 > 0) { + for (i = 0; i < ju; i += ix) { + temp_re_tmp = i + iy; + temp_re = y->data[temp_re_tmp].re; + temp_im = y->data[temp_re_tmp].im; + y->data[temp_re_tmp].re = y->data[i].re - y->data[temp_re_tmp].re; + y->data[temp_re_tmp].im = y->data[i].im - y->data[temp_re_tmp].im; + y->data[i].re += temp_re; + y->data[i].im += temp_im; + } + + istart = 1; + for (nRowsM2 = nRowsD4; nRowsM2 < nRowsD2; nRowsM2 += nRowsD4) { + twid_re = costab->data[nRowsM2]; + twid_im = sintab->data[nRowsM2]; + i = istart; + ihi = istart + ju; + while (i < ihi) { + temp_re_tmp = i + iy; + temp_re = twid_re * y->data[temp_re_tmp].re - twid_im * y-> + data[temp_re_tmp].im; + temp_im = twid_re * y->data[temp_re_tmp].im + twid_im * y-> + data[temp_re_tmp].re; + y->data[temp_re_tmp].re = y->data[i].re - temp_re; + y->data[temp_re_tmp].im = y->data[i].im - temp_im; + y->data[i].re += temp_re; + y->data[i].im += temp_im; + i += ix; + } + + istart++; + } + + nRowsD4 /= 2; + iy = ix; + ix += ix; + ju -= iy; + } +} + +// +// Arguments : const emxArray_creal_T *x +// int N2 +// int n1 +// const emxArray_real_T *costab +// const emxArray_real_T *sintab +// const emxArray_real_T *sintabinv +// emxArray_creal_T *y +// Return Type : void +// +void b_dobluesteinfft(const emxArray_creal_T *x, int N2, int n1, const + emxArray_real_T *costab, const emxArray_real_T *sintab, + const emxArray_real_T *sintabinv, emxArray_creal_T *y) +{ + emxArray_creal_T *wwc; + int nInt2m1; + int i; + int idx; + int rt; + int nInt2; + int k; + int b_y; + double nt_im; + double nt_re; + int b_k; + emxArray_creal_T *fv; + emxArray_creal_T *b_fv; + emxArray_creal_T *rwork; + int b_idx; + int minNrowsNx; + int denom_re_tmp; + int c_k; + double im; + double y_tmp_im; + emxInit_creal_T(&wwc, 1); + nInt2m1 = (n1 + n1) - 1; + i = wwc->size[0]; + wwc->size[0] = nInt2m1; + emxEnsureCapacity_creal_T(wwc, i); + idx = n1; + rt = 0; + wwc->data[n1 - 1].re = 1.0; + wwc->data[n1 - 1].im = 0.0; + nInt2 = n1 << 1; + for (k = 0; k <= n1 - 2; k++) { + b_y = ((k + 1) << 1) - 1; + if (nInt2 - rt <= b_y) { + rt += b_y - nInt2; + } else { + rt += b_y; + } + + nt_im = 3.1415926535897931 * static_cast(rt) / static_cast + (n1); + if (nt_im == 0.0) { + nt_re = 1.0; + nt_im = 0.0; + } else { + nt_re = std::cos(nt_im); + nt_im = std::sin(nt_im); + } + + wwc->data[idx - 2].re = nt_re; + wwc->data[idx - 2].im = -nt_im; + idx--; + } + + idx = 0; + i = nInt2m1 - 1; + for (k = i; k >= n1; k--) { + wwc->data[k] = wwc->data[idx]; + idx++; + } + + nInt2m1 = x->size[0]; + i = y->size[0] * y->size[1]; + y->size[0] = n1; + y->size[1] = x->size[1]; + emxEnsureCapacity_creal_T(y, i); + if (n1 > x->size[0]) { + rt = x->size[1]; + for (i = 0; i < rt; i++) { + nInt2 = y->size[0]; + for (b_y = 0; b_y < nInt2; b_y++) { + y->data[b_y + y->size[0] * i].re = 0.0; + y->data[b_y + y->size[0] * i].im = 0.0; + } + } + } + + rt = x->size[1] - 1; + +#pragma omp parallel \ + num_threads(omp_get_max_threads()) \ + private(fv,b_fv,rwork,b_idx,minNrowsNx,denom_re_tmp,c_k,im,y_tmp_im) + + { + emxInit_creal_T(&fv, 1); + emxInit_creal_T(&b_fv, 1); + emxInit_creal_T(&rwork, 1); + +#pragma omp for nowait + + for (b_k = 0; b_k <= rt; b_k++) { + b_idx = b_k * nInt2m1; + minNrowsNx = x->size[0]; + if (n1 < minNrowsNx) { + minNrowsNx = n1; + } + + denom_re_tmp = rwork->size[0]; + rwork->size[0] = n1; + emxEnsureCapacity_creal_T(rwork, denom_re_tmp); + if (n1 > x->size[0]) { + denom_re_tmp = rwork->size[0]; + rwork->size[0] = n1; + emxEnsureCapacity_creal_T(rwork, denom_re_tmp); + for (denom_re_tmp = 0; denom_re_tmp < n1; denom_re_tmp++) { + rwork->data[denom_re_tmp].re = 0.0; + rwork->data[denom_re_tmp].im = 0.0; + } + } + + for (c_k = 0; c_k < minNrowsNx; c_k++) { + denom_re_tmp = (n1 + c_k) - 1; + rwork->data[c_k].re = wwc->data[denom_re_tmp].re * x->data[b_idx].re + + wwc->data[denom_re_tmp].im * x->data[b_idx].im; + rwork->data[c_k].im = wwc->data[denom_re_tmp].re * x->data[b_idx].im - + wwc->data[denom_re_tmp].im * x->data[b_idx].re; + b_idx++; + } + + denom_re_tmp = minNrowsNx + 1; + for (c_k = denom_re_tmp; c_k <= n1; c_k++) { + rwork->data[c_k - 1].re = 0.0; + rwork->data[c_k - 1].im = 0.0; + } + + r2br_r2dit_trig_impl(rwork, N2, costab, sintab, b_fv); + r2br_r2dit_trig_impl(wwc, N2, costab, sintab, fv); + denom_re_tmp = fv->size[0]; + fv->size[0] = b_fv->size[0]; + emxEnsureCapacity_creal_T(fv, denom_re_tmp); + b_idx = b_fv->size[0]; + for (denom_re_tmp = 0; denom_re_tmp < b_idx; denom_re_tmp++) { + im = b_fv->data[denom_re_tmp].re * fv->data[denom_re_tmp].im + + b_fv->data[denom_re_tmp].im * fv->data[denom_re_tmp].re; + fv->data[denom_re_tmp].re = b_fv->data[denom_re_tmp].re * fv-> + data[denom_re_tmp].re - b_fv->data[denom_re_tmp].im * fv-> + data[denom_re_tmp].im; + fv->data[denom_re_tmp].im = im; + } + + r2br_r2dit_trig_impl(fv, N2, costab, sintabinv, b_fv); + if (b_fv->size[0] > 1) { + im = 1.0 / static_cast(b_fv->size[0]); + b_idx = b_fv->size[0]; + for (denom_re_tmp = 0; denom_re_tmp < b_idx; denom_re_tmp++) { + b_fv->data[denom_re_tmp].re *= im; + b_fv->data[denom_re_tmp].im *= im; + } + } + + b_idx = 0; + denom_re_tmp = wwc->size[0]; + for (c_k = n1; c_k <= denom_re_tmp; c_k++) { + im = wwc->data[c_k - 1].re * b_fv->data[c_k - 1].re + wwc->data[c_k - 1] + .im * b_fv->data[c_k - 1].im; + y_tmp_im = wwc->data[c_k - 1].re * b_fv->data[c_k - 1].im - wwc-> + data[c_k - 1].im * b_fv->data[c_k - 1].re; + rwork->data[b_idx].re = im; + rwork->data[b_idx].im = y_tmp_im; + rwork->data[b_idx].re = im; + rwork->data[b_idx].im = y_tmp_im; + if (rwork->data[b_idx].im == 0.0) { + y_tmp_im = rwork->data[b_idx].re / static_cast(n1); + im = 0.0; + } else if (rwork->data[b_idx].re == 0.0) { + y_tmp_im = 0.0; + im = rwork->data[b_idx].im / static_cast(n1); + } else { + y_tmp_im = rwork->data[b_idx].re / static_cast(n1); + im = rwork->data[b_idx].im / static_cast(n1); + } + + rwork->data[b_idx].re = y_tmp_im; + rwork->data[b_idx].im = im; + b_idx++; + } + + for (denom_re_tmp = 0; denom_re_tmp < n1; denom_re_tmp++) { + b_idx = denom_re_tmp + 1; + y->data[(b_idx + y->size[0] * b_k) - 1] = rwork->data[b_idx - 1]; + } + } + + emxFree_creal_T(&rwork); + emxFree_creal_T(&b_fv); + emxFree_creal_T(&fv); + } + + emxFree_creal_T(&wwc); +} + +// +// Arguments : const emxArray_creal_T *x +// int n1_unsigned +// const emxArray_real_T *costab +// const emxArray_real_T *sintab +// emxArray_creal_T *y +// Return Type : void +// +void b_r2br_r2dit_trig(const emxArray_creal_T *x, int n1_unsigned, const + emxArray_real_T *costab, const emxArray_real_T *sintab, emxArray_creal_T *y) +{ + int n1; + int nrows; + int ub_loop; + int loop_ub; + int k; + int b_loop_ub; + emxArray_creal_T *rwork; + int i; + double b; + int iy; + int xoff; + int ihi; + int nRowsM2; + int nRowsD2; + int nRowsD4; + int ju; + int b_i; + int iDelta2; + boolean_T tst; + double temp_re; + double temp_im; + double twid_re; + double twid_im; + int temp_re_tmp; + n1 = n1_unsigned; + nrows = x->size[0]; + ub_loop = y->size[0] * y->size[1]; + y->size[0] = n1_unsigned; + y->size[1] = x->size[1]; + emxEnsureCapacity_creal_T(y, ub_loop); + if (n1_unsigned > x->size[0]) { + loop_ub = x->size[1]; + for (ub_loop = 0; ub_loop < loop_ub; ub_loop++) { + b_loop_ub = y->size[0]; + for (i = 0; i < b_loop_ub; i++) { + y->data[i + y->size[0] * ub_loop].re = 0.0; + y->data[i + y->size[0] * ub_loop].im = 0.0; + } + } + } + + ub_loop = x->size[1] - 1; + +#pragma omp parallel \ + num_threads(omp_get_max_threads()) \ + private(rwork,iy,xoff,ihi,nRowsM2,nRowsD2,nRowsD4,ju,b_i,iDelta2,tst,temp_re,temp_im,twid_re,twid_im,temp_re_tmp) + + { + emxInit_creal_T(&rwork, 1); + +#pragma omp for nowait + + for (k = 0; k <= ub_loop; k++) { + xoff = k * nrows; + iy = x->size[0]; + ihi = n1_unsigned; + if (iy < n1_unsigned) { + ihi = iy; + } + + nRowsM2 = n1_unsigned - 2; + nRowsD2 = n1_unsigned / 2; + nRowsD4 = nRowsD2 / 2; + iy = rwork->size[0]; + rwork->size[0] = n1_unsigned; + emxEnsureCapacity_creal_T(rwork, iy); + if (n1_unsigned > x->size[0]) { + iy = rwork->size[0]; + rwork->size[0] = n1_unsigned; + emxEnsureCapacity_creal_T(rwork, iy); + for (iy = 0; iy < n1_unsigned; iy++) { + rwork->data[iy].re = 0.0; + rwork->data[iy].im = 0.0; + } + } + + ju = 0; + iy = 0; + for (b_i = 0; b_i <= ihi - 2; b_i++) { + rwork->data[iy] = x->data[xoff]; + iDelta2 = n1_unsigned; + tst = true; + while (tst) { + iDelta2 >>= 1; + ju ^= iDelta2; + tst = ((ju & iDelta2) == 0); + } + + iy = ju; + xoff++; + } + + rwork->data[iy] = x->data[xoff]; + if (n1_unsigned > 1) { + for (b_i = 0; b_i <= nRowsM2; b_i += 2) { + temp_re = rwork->data[b_i + 1].re; + temp_im = rwork->data[b_i + 1].im; + twid_re = rwork->data[b_i].re; + twid_im = rwork->data[b_i].im; + rwork->data[b_i + 1].re = rwork->data[b_i].re - rwork->data[b_i + 1]. + re; + rwork->data[b_i + 1].im = rwork->data[b_i].im - rwork->data[b_i + 1]. + im; + twid_re += temp_re; + twid_im += temp_im; + rwork->data[b_i].re = twid_re; + rwork->data[b_i].im = twid_im; + } + } + + iy = 2; + iDelta2 = 4; + ju = ((nRowsD4 - 1) << 2) + 1; + while (nRowsD4 > 0) { + for (b_i = 0; b_i < ju; b_i += iDelta2) { + temp_re_tmp = b_i + iy; + temp_re = rwork->data[temp_re_tmp].re; + temp_im = rwork->data[temp_re_tmp].im; + rwork->data[temp_re_tmp].re = rwork->data[b_i].re - rwork-> + data[temp_re_tmp].re; + rwork->data[temp_re_tmp].im = rwork->data[b_i].im - rwork-> + data[temp_re_tmp].im; + rwork->data[b_i].re += temp_re; + rwork->data[b_i].im += temp_im; + } + + nRowsM2 = 1; + for (xoff = nRowsD4; xoff < nRowsD2; xoff += nRowsD4) { + twid_re = costab->data[xoff]; + twid_im = sintab->data[xoff]; + b_i = nRowsM2; + ihi = nRowsM2 + ju; + while (b_i < ihi) { + temp_re_tmp = b_i + iy; + temp_re = twid_re * rwork->data[temp_re_tmp].re - twid_im * + rwork->data[temp_re_tmp].im; + temp_im = twid_re * rwork->data[temp_re_tmp].im + twid_im * + rwork->data[temp_re_tmp].re; + rwork->data[temp_re_tmp].re = rwork->data[b_i].re - temp_re; + rwork->data[temp_re_tmp].im = rwork->data[b_i].im - temp_im; + rwork->data[b_i].re += temp_re; + rwork->data[b_i].im += temp_im; + b_i += iDelta2; + } + + nRowsM2++; + } + + nRowsD4 /= 2; + iy = iDelta2; + iDelta2 += iDelta2; + ju -= iy; + } + + for (iDelta2 = 0; iDelta2 < n1; iDelta2++) { + iy = iDelta2 + 1; + y->data[(iy + y->size[0] * k) - 1] = rwork->data[iy - 1]; + } + } + + emxFree_creal_T(&rwork); + } + + if (y->size[0] > 1) { + b = 1.0 / static_cast(y->size[0]); + loop_ub = y->size[0] * y->size[1]; + for (ub_loop = 0; ub_loop < loop_ub; ub_loop++) { + y->data[ub_loop].re *= b; + y->data[ub_loop].im *= b; + } + } +} + +// +// Arguments : const double x_data[] +// const int x_size[1] +// int N2 +// int n1 +// const double costab_data[] +// const int costab_size[2] +// const double sintab_data[] +// const int sintab_size[2] +// const double sintabinv_data[] +// const int sintabinv_size[2] +// creal_T y_data[] +// int y_size[1] +// Return Type : void +// +void dobluesteinfft(const double x_data[], const int x_size[1], int N2, int n1, + const double costab_data[], const int costab_size[2], const + double sintab_data[], const int sintab_size[2], const double + sintabinv_data[], const int sintabinv_size[2], creal_T + y_data[], int y_size[1]) +{ + emxArray_creal_T *wwc; + int nInt2m1; + int rt; + int idx; + int nInt2; + int k; + int y; + double nt_im; + double nt_re; + emxInit_creal_T(&wwc, 1); + nInt2m1 = (n1 + n1) - 1; + rt = wwc->size[0]; + wwc->size[0] = nInt2m1; + emxEnsureCapacity_creal_T(wwc, rt); + idx = n1; + rt = 0; + wwc->data[n1 - 1].re = 1.0; + wwc->data[n1 - 1].im = 0.0; + nInt2 = n1 << 1; + for (k = 0; k <= n1 - 2; k++) { + y = ((k + 1) << 1) - 1; + if (nInt2 - rt <= y) { + rt += y - nInt2; + } else { + rt += y; + } + + nt_im = -3.1415926535897931 * static_cast(rt) / static_cast + (n1); + if (nt_im == 0.0) { + nt_re = 1.0; + nt_im = 0.0; + } else { + nt_re = std::cos(nt_im); + nt_im = std::sin(nt_im); + } + + wwc->data[idx - 2].re = nt_re; + wwc->data[idx - 2].im = -nt_im; + idx--; + } + + idx = 0; + rt = nInt2m1 - 1; + for (k = rt; k >= n1; k--) { + wwc->data[k] = wwc->data[idx]; + idx++; + } + + bluestein(x_data, x_size, N2, n1, costab_data, costab_size, sintab_data, + sintab_size, costab_data, costab_size, sintabinv_data, + sintabinv_size, wwc, y_data, y_size); + emxFree_creal_T(&wwc); +} + +// +// Arguments : int nRows +// boolean_T useRadix2 +// double costab_data[] +// int costab_size[2] +// double sintab_data[] +// int sintab_size[2] +// double sintabinv_data[] +// int sintabinv_size[2] +// Return Type : void +// +void generate_twiddle_tables(int nRows, boolean_T useRadix2, double costab_data[], + int costab_size[2], double sintab_data[], int sintab_size[2], double + sintabinv_data[], int sintabinv_size[2]) +{ + emxArray_real_T *costab1q; + double e; + int n; + int i; + int nd2; + int k; + emxArray_real_T *costab; + emxArray_real_T *sintab; + emxArray_real_T *sintabinv; + emxInit_real_T(&costab1q, 2); + e = 6.2831853071795862 / static_cast(nRows); + n = nRows / 2 / 2; + i = costab1q->size[0] * costab1q->size[1]; + costab1q->size[0] = 1; + costab1q->size[1] = n + 1; + emxEnsureCapacity_real_T(costab1q, i); + costab1q->data[0] = 1.0; + nd2 = n / 2 - 1; + for (k = 0; k <= nd2; k++) { + costab1q->data[k + 1] = std::cos(e * (static_cast(k) + 1.0)); + } + + i = nd2 + 2; + nd2 = n - 1; + for (k = i; k <= nd2; k++) { + costab1q->data[k] = std::sin(e * static_cast((n - k))); + } + + costab1q->data[n] = 0.0; + if (!useRadix2) { + emxInit_real_T(&costab, 2); + emxInit_real_T(&sintab, 2); + emxInit_real_T(&sintabinv, 2); + n = costab1q->size[1] - 1; + nd2 = (costab1q->size[1] - 1) << 1; + i = costab->size[0] * costab->size[1]; + costab->size[0] = 1; + costab->size[1] = nd2 + 1; + emxEnsureCapacity_real_T(costab, i); + i = sintab->size[0] * sintab->size[1]; + sintab->size[0] = 1; + sintab->size[1] = nd2 + 1; + emxEnsureCapacity_real_T(sintab, i); + costab->data[0] = 1.0; + sintab->data[0] = 0.0; + i = sintabinv->size[0] * sintabinv->size[1]; + sintabinv->size[0] = 1; + sintabinv->size[1] = nd2 + 1; + emxEnsureCapacity_real_T(sintabinv, i); + for (k = 0; k < n; k++) { + sintabinv->data[k + 1] = costab1q->data[(n - k) - 1]; + } + + i = costab1q->size[1]; + for (k = i; k <= nd2; k++) { + sintabinv->data[k] = costab1q->data[k - n]; + } + + for (k = 0; k < n; k++) { + costab->data[k + 1] = costab1q->data[k + 1]; + sintab->data[k + 1] = -costab1q->data[(n - k) - 1]; + } + + i = costab1q->size[1]; + for (k = i; k <= nd2; k++) { + costab->data[k] = -costab1q->data[nd2 - k]; + sintab->data[k] = -costab1q->data[k - n]; + } + + costab_size[0] = 1; + costab_size[1] = costab->size[1]; + nd2 = costab->size[0] * costab->size[1]; + for (i = 0; i < nd2; i++) { + costab_data[i] = costab->data[i]; + } + + emxFree_real_T(&costab); + sintab_size[0] = 1; + sintab_size[1] = sintab->size[1]; + nd2 = sintab->size[0] * sintab->size[1]; + for (i = 0; i < nd2; i++) { + sintab_data[i] = sintab->data[i]; + } + + emxFree_real_T(&sintab); + sintabinv_size[0] = 1; + sintabinv_size[1] = sintabinv->size[1]; + nd2 = sintabinv->size[0] * sintabinv->size[1]; + for (i = 0; i < nd2; i++) { + sintabinv_data[i] = sintabinv->data[i]; + } + + emxFree_real_T(&sintabinv); + } else { + n = costab1q->size[1] - 1; + nd2 = (costab1q->size[1] - 1) << 1; + costab_size[0] = 1; + costab_size[1] = nd2 + 1; + sintab_size[0] = 1; + sintab_size[1] = nd2 + 1; + costab_data[0] = 1.0; + sintab_data[0] = 0.0; + for (k = 0; k < n; k++) { + costab_data[k + 1] = costab1q->data[k + 1]; + sintab_data[k + 1] = -costab1q->data[(n - k) - 1]; + } + + i = costab1q->size[1]; + for (k = i; k <= nd2; k++) { + costab_data[k] = -costab1q->data[nd2 - k]; + sintab_data[k] = -costab1q->data[k - n]; + } + + sintabinv_size[0] = 1; + sintabinv_size[1] = 0; + } + + emxFree_real_T(&costab1q); +} + +// +// Arguments : int n1 +// boolean_T useRadix2 +// int *N2blue +// int *nRows +// Return Type : void +// +void get_algo_sizes(int n1, boolean_T useRadix2, int *N2blue, int *nRows) +{ + int n; + int pmax; + int pmin; + boolean_T exitg1; + int k; + int pow2p; + *N2blue = 1; + if (useRadix2) { + *nRows = n1; + } else { + n = (n1 + n1) - 1; + pmax = 31; + if (n <= 1) { + pmax = 0; + } else { + pmin = 0; + exitg1 = false; + while ((!exitg1) && (pmax - pmin > 1)) { + k = (pmin + pmax) >> 1; + pow2p = 1 << k; + if (pow2p == n) { + pmax = k; + exitg1 = true; + } else if (pow2p > n) { + pmax = k; + } else { + pmin = k; + } + } + } + + *N2blue = 1 << pmax; + *nRows = *N2blue; + } +} + +// +// Arguments : const double x_data[] +// const int x_size[1] +// int n1_unsigned +// const double costab_data[] +// const double sintab_data[] +// creal_T y_data[] +// int y_size[1] +// Return Type : void +// +void r2br_r2dit_trig(const double x_data[], const int x_size[1], int n1_unsigned, + const double costab_data[], const double sintab_data[], + creal_T y_data[], int y_size[1]) +{ + int iDelta2; + int nRowsM2; + int nRowsD2; + int nRowsD4; + int ix; + int ju; + int iy; + int i; + boolean_T tst; + double twid_re; + double temp_re; + double twid_im; + double temp_im; + double re; + double im; + int temp_re_tmp; + int ihi; + iDelta2 = x_size[0]; + if (iDelta2 >= n1_unsigned) { + iDelta2 = n1_unsigned; + } + + nRowsM2 = n1_unsigned - 2; + nRowsD2 = n1_unsigned / 2; + nRowsD4 = nRowsD2 / 2; + y_size[0] = n1_unsigned; + if (n1_unsigned > x_size[0]) { + y_size[0] = n1_unsigned; + if (0 <= n1_unsigned - 1) { + std::memset(&y_data[0], 0, n1_unsigned * sizeof(creal_T)); + } + } + + ix = 0; + ju = 0; + iy = 0; + for (i = 0; i <= iDelta2 - 2; i++) { + y_data[iy].re = x_data[ix]; + y_data[iy].im = 0.0; + iy = n1_unsigned; + tst = true; + while (tst) { + iy >>= 1; + ju ^= iy; + tst = ((ju & iy) == 0); + } + + iy = ju; + ix++; + } + + y_data[iy].re = x_data[ix]; + y_data[iy].im = 0.0; + if (n1_unsigned > 1) { + for (i = 0; i <= nRowsM2; i += 2) { + twid_re = y_data[i + 1].re; + temp_re = twid_re; + twid_im = y_data[i + 1].im; + temp_im = twid_im; + re = y_data[i].re; + im = y_data[i].im; + twid_re = y_data[i].re - twid_re; + y_data[i + 1].re = twid_re; + twid_im = y_data[i].im - twid_im; + y_data[i + 1].im = twid_im; + y_data[i].re = re + temp_re; + y_data[i].im = im + temp_im; + } + } + + iy = 2; + iDelta2 = 4; + nRowsM2 = ((nRowsD4 - 1) << 2) + 1; + while (nRowsD4 > 0) { + for (i = 0; i < nRowsM2; i += iDelta2) { + temp_re_tmp = i + iy; + temp_re = y_data[temp_re_tmp].re; + temp_im = y_data[temp_re_tmp].im; + y_data[temp_re_tmp].re = y_data[i].re - y_data[temp_re_tmp].re; + y_data[temp_re_tmp].im = y_data[i].im - y_data[temp_re_tmp].im; + y_data[i].re += temp_re; + y_data[i].im += temp_im; + } + + ix = 1; + for (ju = nRowsD4; ju < nRowsD2; ju += nRowsD4) { + twid_re = costab_data[ju]; + twid_im = sintab_data[ju]; + i = ix; + ihi = ix + nRowsM2; + while (i < ihi) { + temp_re_tmp = i + iy; + temp_re = twid_re * y_data[temp_re_tmp].re - twid_im * + y_data[temp_re_tmp].im; + temp_im = twid_re * y_data[temp_re_tmp].im + twid_im * + y_data[temp_re_tmp].re; + y_data[temp_re_tmp].re = y_data[i].re - temp_re; + y_data[temp_re_tmp].im = y_data[i].im - temp_im; + y_data[i].re += temp_re; + y_data[i].im += temp_im; + i += iDelta2; + } + + ix++; + } + + nRowsD4 /= 2; + iy = iDelta2; + iDelta2 += iDelta2; + nRowsM2 -= iy; + } +} + +// +// File trailer for fft1.cpp +// +// [EOF] +// diff --git a/Classes/fft1.h b/Classes/fft1.h new file mode 100644 index 0000000..816f03d --- /dev/null +++ b/Classes/fft1.h @@ -0,0 +1,41 @@ +// +// File: fft1.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef FFT1_H +#define FFT1_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void b_dobluesteinfft(const emxArray_creal_T *x, int N2, int n1, const + emxArray_real_T *costab, const emxArray_real_T *sintab, const emxArray_real_T * + sintabinv, emxArray_creal_T *y); +extern void b_r2br_r2dit_trig(const emxArray_creal_T *x, int n1_unsigned, const + emxArray_real_T *costab, const emxArray_real_T *sintab, emxArray_creal_T *y); +extern void dobluesteinfft(const double x_data[], const int x_size[1], int N2, + int n1, const double costab_data[], const int costab_size[2], const double + sintab_data[], const int sintab_size[2], const double sintabinv_data[], const + int sintabinv_size[2], creal_T y_data[], int y_size[1]); +extern void generate_twiddle_tables(int nRows, boolean_T useRadix2, double + costab_data[], int costab_size[2], double sintab_data[], int sintab_size[2], + double sintabinv_data[], int sintabinv_size[2]); +extern void get_algo_sizes(int n1, boolean_T useRadix2, int *N2blue, int *nRows); +extern void r2br_r2dit_trig(const double x_data[], const int x_size[1], int + n1_unsigned, const double costab_data[], const double sintab_data[], creal_T + y_data[], int y_size[1]); + +#endif + +// +// File trailer for fft1.h +// +// [EOF] +// diff --git a/Classes/fstools.cpp b/Classes/fstools.cpp new file mode 100644 index 0000000..ac4c5e3 --- /dev/null +++ b/Classes/fstools.cpp @@ -0,0 +1,31 @@ +#include "fstools.h" +#include + +namespace fstools{ + bool delDir(const QString &path) { + if (path.isEmpty()){ + return false; + } + QDir dir(path); + if(!dir.exists()){ + return true; + } + dir.setFilter(QDir::AllEntries | QDir::NoDotAndDotDot); + QFileInfoList fileList = dir.entryInfoList(); + foreach (QFileInfo file, fileList){ + if (file.isFile()) { + file.dir().remove(file.fileName()); + } + else { + delDir(file.absoluteFilePath()); + } + } + return dir.rmpath(dir.absolutePath()); + } + bool mkDir(const QString &path, const QString &dirName) { + QDir dir(path); + dir.cd(path); + bool ok = dir.mkdir(dirName); + return ok; + } +} diff --git a/Classes/fstools.h b/Classes/fstools.h new file mode 100644 index 0000000..faf2cd2 --- /dev/null +++ b/Classes/fstools.h @@ -0,0 +1,13 @@ +#ifndef FSTOOLS_H +#define FSTOOLS_H + + +class QString; + +namespace fstools +{ + bool delDir(const QString &path); + bool mkDir(const QString &path, const QString &dirName); +} + +#endif // FSTOOLS_H diff --git a/Classes/gammaln.cpp b/Classes/gammaln.cpp new file mode 100644 index 0000000..45f7531 --- /dev/null +++ b/Classes/gammaln.cpp @@ -0,0 +1,163 @@ +// +// File: gammaln.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "gammaln.h" +#include "cwt.h" +#include "rt_nonfinite.h" +#include +#include + +// Function Definitions + +// +// Arguments : double *x +// Return Type : void +// +void gammaln(double *x) +{ + double t; + double r; + static const double table100[100] = { 0.0, 0.0, 0.69314718055994529, + 1.791759469228055, 3.1780538303479458, 4.7874917427820458, + 6.5792512120101012, 8.5251613610654147, 10.604602902745251, + 12.801827480081469, 15.104412573075516, 17.502307845873887, + 19.987214495661885, 22.552163853123425, 25.19122118273868, 27.89927138384089, + 30.671860106080672, 33.505073450136891, 36.395445208033053, + 39.339884187199495, 42.335616460753485, 45.380138898476908, + 48.471181351835227, 51.606675567764377, 54.784729398112319, + 58.003605222980518, 61.261701761002, 64.557538627006338, 67.88974313718154, + 71.257038967168015, 74.658236348830158, 78.0922235533153, 81.557959456115043, + 85.054467017581516, 88.580827542197682, 92.1361756036871, 95.7196945421432, + 99.330612454787428, 102.96819861451381, 106.63176026064346, + 110.32063971475739, 114.03421178146171, 117.77188139974507, + 121.53308151543864, 125.3172711493569, 129.12393363912722, + 132.95257503561632, 136.80272263732635, 140.67392364823425, + 144.5657439463449, 148.47776695177302, 152.40959258449735, 156.3608363030788, + 160.3311282166309, 164.32011226319517, 168.32744544842765, + 172.35279713916279, 176.39584840699735, 180.45629141754378, + 184.53382886144948, 188.6281734236716, 192.7390472878449, 196.86618167289, + 201.00931639928152, 205.1681994826412, 209.34258675253685, + 213.53224149456327, 217.73693411395422, 221.95644181913033, + 226.1905483237276, 230.43904356577696, 234.70172344281826, + 238.97838956183432, 243.26884900298271, 247.57291409618688, + 251.89040220972319, 256.22113555000954, 260.56494097186322, + 264.92164979855278, 269.29109765101981, 273.67312428569369, + 278.06757344036612, 282.4742926876304, 286.893133295427, 291.32395009427029, + 295.76660135076065, 300.22094864701415, 304.68685676566872, + 309.1641935801469, 313.65282994987905, 318.1526396202093, 322.66349912672615, + 327.1852877037752, 331.71788719692847, 336.26118197919845, 340.815058870799, + 345.37940706226686, 349.95411804077025, 354.53908551944079, + 359.1342053695754 }; + + int i; + static const double p1[8] = { 4.9452353592967269, 201.8112620856775, + 2290.8383738313464, 11319.672059033808, 28557.246356716354, + 38484.962284437934, 26377.487876241954, 7225.8139797002877 }; + + static const double p2[8] = { 4.974607845568932, 542.4138599891071, + 15506.938649783649, 184793.29044456323, 1.0882047694688288E+6, + 3.33815296798703E+6, 5.1066616789273527E+6, 3.0741090548505397E+6 }; + + static const double q1[8] = { 67.482125503037778, 1113.3323938571993, + 7738.7570569353984, 27639.870744033407, 54993.102062261576, + 61611.221800660023, 36351.2759150194, 8785.5363024310136 }; + + static const double q2[8] = { 183.03283993705926, 7765.0493214450062, + 133190.38279660742, 1.1367058213219696E+6, 5.2679641174379466E+6, + 1.3467014543111017E+7, 1.7827365303532742E+7, 9.5330955918443538E+6 }; + + static const double p4[8] = { 14745.0216605994, 2.4268133694867045E+6, + 1.2147555740450932E+8, 2.6634324496309772E+9, 2.9403789566345539E+10, + 1.7026657377653989E+11, 4.926125793377431E+11, 5.6062518562239514E+11 }; + + static const double c[7] = { -0.001910444077728, 0.00084171387781295, + -0.00059523799130430121, 0.0007936507935003503, -0.0027777777777776816, + 0.083333333333333329, 0.0057083835261 }; + + static const double q4[8] = { 2690.5301758708993, 639388.56543000927, + 4.1355999302413881E+7, 1.120872109616148E+9, 1.4886137286788137E+10, + 1.0168035862724382E+11, 3.4174763455073773E+11, 4.4631581874197131E+11 }; + + if ((!rtIsNaN(*x)) && (!(*x < 0.0))) { + if (*x > 2.55E+305) { + *x = rtInf; + } else if (*x <= 2.2204460492503131E-16) { + *x = -std::log(*x); + } else if (*x <= 0.5) { + t = 1.0; + r = 0.0; + for (i = 0; i < 8; i++) { + r = r * *x + p1[i]; + t = t * *x + q1[i]; + } + + *x = -std::log(*x) + *x * (*x * (r / t) + -0.57721566490153287); + } else if (*x <= 0.6796875) { + t = 1.0; + r = 0.0; + for (i = 0; i < 8; i++) { + r = r * ((*x - 0.5) - 0.5) + p2[i]; + t = t * ((*x - 0.5) - 0.5) + q2[i]; + } + + *x = -std::log(*x) + ((*x - 0.5) - 0.5) * (((*x - 0.5) - 0.5) * (r / t) + + 0.42278433509846713); + } else if ((*x == std::floor(*x)) && (*x <= 100.0)) { + *x = table100[static_cast(*x) - 1]; + } else if (*x <= 1.5) { + t = 1.0; + r = 0.0; + for (i = 0; i < 8; i++) { + r = r * ((*x - 0.5) - 0.5) + p1[i]; + t = t * ((*x - 0.5) - 0.5) + q1[i]; + } + + *x = ((*x - 0.5) - 0.5) * (((*x - 0.5) - 0.5) * (r / t) + + -0.57721566490153287); + } else if (*x <= 4.0) { + t = 1.0; + r = 0.0; + for (i = 0; i < 8; i++) { + r = r * (*x - 2.0) + p2[i]; + t = t * (*x - 2.0) + q2[i]; + } + + *x = (*x - 2.0) * ((*x - 2.0) * (r / t) + 0.42278433509846713); + } else if (*x <= 12.0) { + t = -1.0; + r = 0.0; + for (i = 0; i < 8; i++) { + r = r * (*x - 4.0) + p4[i]; + t = t * (*x - 4.0) + q4[i]; + } + + *x = (*x - 4.0) * (r / t) + 1.791759469228055; + } else { + if (*x <= 2.25E+76) { + r = 0.0057083835261; + t = 1.0 / (*x * *x); + for (i = 0; i < 6; i++) { + r = r * t + c[i]; + } + + r /= *x; + } else { + r = 0.0; + } + + t = std::log(*x); + *x = ((r + 0.91893853320467278) - 0.5 * t) + *x * (t - 1.0); + } + } +} + +// +// File trailer for gammaln.cpp +// +// [EOF] +// diff --git a/Classes/gammaln.h b/Classes/gammaln.h new file mode 100644 index 0000000..f064e27 --- /dev/null +++ b/Classes/gammaln.h @@ -0,0 +1,26 @@ +// +// File: gammaln.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef GAMMALN_H +#define GAMMALN_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void gammaln(double *x); + +#endif + +// +// File trailer for gammaln.h +// +// [EOF] +// diff --git a/Classes/hid.Win.c b/Classes/hid.Win.c new file mode 100644 index 0000000..4b0465b --- /dev/null +++ b/Classes/hid.Win.c @@ -0,0 +1,936 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + Alan Ott + Signal 11 Software + 8/22/2009 + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ + +#include + +#ifndef _NTDEF_ +typedef LONG NTSTATUS; +#endif + +#ifdef __MINGW32__ +#include +#include +#endif + +#ifdef __CYGWIN__ +#include +#define _wcsdup wcsdup +#endif + +/*#define HIDAPI_USE_DDK*/ + +#ifdef __cplusplus +extern "C" { +#endif + #include + #include + #ifdef HIDAPI_USE_DDK + #include + #endif + + /* Copied from inc/ddk/hidclass.h, part of the Windows DDK. */ + #define HID_OUT_CTL_CODE(id) \ + CTL_CODE(FILE_DEVICE_KEYBOARD, (id), METHOD_OUT_DIRECT, FILE_ANY_ACCESS) + #define IOCTL_HID_GET_FEATURE HID_OUT_CTL_CODE(100) + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#include +#include + + +#include "hidapi.h" + +#ifdef _MSC_VER + /* Thanks Microsoft, but I know how to use strncpy(). */ + #pragma warning(disable:4996) +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HIDAPI_USE_DDK + /* Since we're not building with the DDK, and the HID header + files aren't part of the SDK, we have to define all this + stuff here. In lookup_functions(), the function pointers + defined below are set. */ + typedef struct _HIDD_ATTRIBUTES{ + ULONG Size; + USHORT VendorID; + USHORT ProductID; + USHORT VersionNumber; + } HIDD_ATTRIBUTES, *PHIDD_ATTRIBUTES; + + typedef USHORT USAGE; + typedef struct _HIDP_CAPS { + USAGE Usage; + USAGE UsagePage; + USHORT InputReportByteLength; + USHORT OutputReportByteLength; + USHORT FeatureReportByteLength; + USHORT Reserved[17]; + USHORT fields_not_used_by_hidapi[10]; + } HIDP_CAPS, *PHIDP_CAPS; + typedef void* PHIDP_PREPARSED_DATA; + #define HIDP_STATUS_SUCCESS 0x110000 + + typedef BOOLEAN (__stdcall *HidD_GetAttributes_)(HANDLE device, PHIDD_ATTRIBUTES attrib); + typedef BOOLEAN (__stdcall *HidD_GetSerialNumberString_)(HANDLE device, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_GetManufacturerString_)(HANDLE handle, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_GetProductString_)(HANDLE handle, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_SetFeature_)(HANDLE handle, PVOID data, ULONG length); + typedef BOOLEAN (__stdcall *HidD_GetFeature_)(HANDLE handle, PVOID data, ULONG length); + typedef BOOLEAN (__stdcall *HidD_GetIndexedString_)(HANDLE handle, ULONG string_index, PVOID buffer, ULONG buffer_len); + typedef BOOLEAN (__stdcall *HidD_GetPreparsedData_)(HANDLE handle, PHIDP_PREPARSED_DATA *preparsed_data); + typedef BOOLEAN (__stdcall *HidD_FreePreparsedData_)(PHIDP_PREPARSED_DATA preparsed_data); + typedef NTSTATUS (__stdcall *HidP_GetCaps_)(PHIDP_PREPARSED_DATA preparsed_data, HIDP_CAPS *caps); + typedef BOOLEAN (__stdcall *HidD_SetNumInputBuffers_)(HANDLE handle, ULONG number_buffers); + + static HidD_GetAttributes_ HidD_GetAttributes; + static HidD_GetSerialNumberString_ HidD_GetSerialNumberString; + static HidD_GetManufacturerString_ HidD_GetManufacturerString; + static HidD_GetProductString_ HidD_GetProductString; + static HidD_SetFeature_ HidD_SetFeature; + static HidD_GetFeature_ HidD_GetFeature; + static HidD_GetIndexedString_ HidD_GetIndexedString; + static HidD_GetPreparsedData_ HidD_GetPreparsedData; + static HidD_FreePreparsedData_ HidD_FreePreparsedData; + static HidP_GetCaps_ HidP_GetCaps; + static HidD_SetNumInputBuffers_ HidD_SetNumInputBuffers; + + static HMODULE lib_handle = NULL; + static BOOLEAN initialized = FALSE; +#endif /* HIDAPI_USE_DDK */ + +struct hid_device_ { + HANDLE device_handle; + BOOL blocking; + USHORT output_report_length; + size_t input_report_length; + void *last_error_str; + DWORD last_error_num; + BOOL read_pending; + char *read_buf; + OVERLAPPED ol; +}; + +static hid_device *new_hid_device() +{ + hid_device *dev = (hid_device*) calloc(1, sizeof(hid_device)); + dev->device_handle = INVALID_HANDLE_VALUE; + dev->blocking = TRUE; + dev->output_report_length = 0; + dev->input_report_length = 0; + dev->last_error_str = NULL; + dev->last_error_num = 0; + dev->read_pending = FALSE; + dev->read_buf = NULL; + memset(&dev->ol, 0, sizeof(dev->ol)); + dev->ol.hEvent = CreateEvent(NULL, FALSE, FALSE /*inital state f=nonsignaled*/, NULL); + + return dev; +} + +static void free_hid_device(hid_device *dev) +{ + CloseHandle(dev->ol.hEvent); + CloseHandle(dev->device_handle); + LocalFree(dev->last_error_str); + free(dev->read_buf); + free(dev); +} + +static void register_error(hid_device *device, const char *op) +{ + WCHAR *ptr, *msg; + + FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER | + FORMAT_MESSAGE_FROM_SYSTEM | + FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, + GetLastError(), + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + (LPVOID)&msg, 0/*sz*/, + NULL); + + /* Get rid of the CR and LF that FormatMessage() sticks at the + end of the message. Thanks Microsoft! */ + ptr = msg; + while (*ptr) { + if (*ptr == '\r') { + *ptr = 0x0000; + break; + } + ptr++; + } + + /* Store the message off in the Device entry so that + the hid_error() function can pick it up. */ + LocalFree(device->last_error_str); + device->last_error_str = msg; +} + +#ifndef HIDAPI_USE_DDK +static int lookup_functions() +{ + lib_handle = LoadLibraryA("hid.dll"); + if (lib_handle) { +#define RESOLVE(x) x = (x##_)GetProcAddress(lib_handle, #x); if (!x) return -1; + RESOLVE(HidD_GetAttributes); + RESOLVE(HidD_GetSerialNumberString); + RESOLVE(HidD_GetManufacturerString); + RESOLVE(HidD_GetProductString); + RESOLVE(HidD_SetFeature); + RESOLVE(HidD_GetFeature); + RESOLVE(HidD_GetIndexedString); + RESOLVE(HidD_GetPreparsedData); + RESOLVE(HidD_FreePreparsedData); + RESOLVE(HidP_GetCaps); + RESOLVE(HidD_SetNumInputBuffers); +#undef RESOLVE + } + else + return -1; + + return 0; +} +#endif + +static HANDLE open_device(const char *path, BOOL enumerate) +{ + HANDLE handle; + DWORD desired_access = (enumerate)? 0: (GENERIC_WRITE | GENERIC_READ); + DWORD share_mode = (enumerate)? + FILE_SHARE_READ|FILE_SHARE_WRITE: + FILE_SHARE_READ; + + handle = CreateFileA(path, + desired_access, + share_mode, + NULL, + OPEN_EXISTING, + FILE_FLAG_OVERLAPPED,/*FILE_ATTRIBUTE_NORMAL,*/ + 0); + + return handle; +} + +int HID_API_EXPORT hid_init(void) +{ +#ifndef HIDAPI_USE_DDK + if (!initialized) { + if (lookup_functions() < 0) { + hid_exit(); + return -1; + } + initialized = TRUE; + } +#endif + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ +#ifndef HIDAPI_USE_DDK + if (lib_handle) + FreeLibrary(lib_handle); + lib_handle = NULL; + initialized = FALSE; +#endif + return 0; +} + +struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + BOOL res; + struct hid_device_info *root = NULL; /* return object */ + struct hid_device_info *cur_dev = NULL; + + /* Windows objects for interacting with the driver. */ + GUID InterfaceClassGuid = {0x4d1e55b2, 0xf16f, 0x11cf, {0x88, 0xcb, 0x00, 0x11, 0x11, 0x00, 0x00, 0x30} }; + SP_DEVINFO_DATA devinfo_data; + SP_DEVICE_INTERFACE_DATA device_interface_data; + SP_DEVICE_INTERFACE_DETAIL_DATA_A *device_interface_detail_data = NULL; + HDEVINFO device_info_set = INVALID_HANDLE_VALUE; + int device_index = 0; + int i; + + if (hid_init() < 0) + return NULL; + + /* Initialize the Windows objects. */ + memset(&devinfo_data, 0x0, sizeof(devinfo_data)); + devinfo_data.cbSize = sizeof(SP_DEVINFO_DATA); + device_interface_data.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + + /* Get information for all the devices belonging to the HID class. */ + device_info_set = SetupDiGetClassDevsA(&InterfaceClassGuid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + + /* Iterate over each device in the HID class, looking for the right one. */ + + for (;;) { + HANDLE write_handle = INVALID_HANDLE_VALUE; + DWORD required_size = 0; + HIDD_ATTRIBUTES attrib; + + res = SetupDiEnumDeviceInterfaces(device_info_set, + NULL, + &InterfaceClassGuid, + device_index, + &device_interface_data); + + if (!res) { + /* A return of FALSE from this function means that + there are no more devices. */ + break; + } + + /* Call with 0-sized detail size, and let the function + tell us how long the detail struct needs to be. The + size is put in &required_size. */ + res = SetupDiGetDeviceInterfaceDetailA(device_info_set, + &device_interface_data, + NULL, + 0, + &required_size, + NULL); + + /* Allocate a long enough structure for device_interface_detail_data. */ + device_interface_detail_data = (SP_DEVICE_INTERFACE_DETAIL_DATA_A*) malloc(required_size); + device_interface_detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); + + /* Get the detailed data for this device. The detail data gives us + the device path for this device, which is then passed into + CreateFile() to get a handle to the device. */ + res = SetupDiGetDeviceInterfaceDetailA(device_info_set, + &device_interface_data, + device_interface_detail_data, + required_size, + NULL, + NULL); + + if (!res) { + /* register_error(dev, "Unable to call SetupDiGetDeviceInterfaceDetail"); + Continue to the next device. */ + goto cont; + } + + /* Make sure this device is of Setup Class "HIDClass" and has a + driver bound to it. */ + for (i = 0; ; i++) { + char driver_name[256]; + + /* Populate devinfo_data. This function will return failure + when there are no more interfaces left. */ + res = SetupDiEnumDeviceInfo(device_info_set, i, &devinfo_data); + if (!res) + goto cont; + + res = SetupDiGetDeviceRegistryPropertyA(device_info_set, &devinfo_data, + SPDRP_CLASS, NULL, (PBYTE)driver_name, sizeof(driver_name), NULL); + if (!res) + goto cont; + + if (strcmp(driver_name, "HIDClass") == 0) { + /* See if there's a driver bound. */ + res = SetupDiGetDeviceRegistryPropertyA(device_info_set, &devinfo_data, + SPDRP_DRIVER, NULL, (PBYTE)driver_name, sizeof(driver_name), NULL); + if (res) + break; + } + } + + //wprintf(L"HandleName: %s\n", device_interface_detail_data->DevicePath); + + /* Open a handle to the device */ + write_handle = open_device(device_interface_detail_data->DevicePath, TRUE); + + /* Check validity of write_handle. */ + if (write_handle == INVALID_HANDLE_VALUE) { + /* Unable to open the device. */ + //register_error(dev, "CreateFile"); + goto cont_close; + } + + + /* Get the Vendor ID and Product ID for this device. */ + attrib.Size = sizeof(HIDD_ATTRIBUTES); + HidD_GetAttributes(write_handle, &attrib); + //wprintf(L"Product/Vendor: %x %x\n", attrib.ProductID, attrib.VendorID); + + /* Check the VID/PID to see if we should add this + device to the enumeration list. */ + if ((vendor_id == 0x0 || attrib.VendorID == vendor_id) && + (product_id == 0x0 || attrib.ProductID == product_id)) { + + #define WSTR_LEN 512 + const char *str; + struct hid_device_info *tmp; + PHIDP_PREPARSED_DATA pp_data = NULL; + HIDP_CAPS caps; + BOOLEAN res; + NTSTATUS nt_res; + wchar_t wstr[WSTR_LEN]; /* TODO: Determine Size */ + size_t len; + + /* VID/PID match. Create the record. */ + tmp = (struct hid_device_info*) calloc(1, sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + cur_dev = tmp; + + /* Get the Usage Page and Usage for this device. */ + res = HidD_GetPreparsedData(write_handle, &pp_data); + if (res) { + nt_res = HidP_GetCaps(pp_data, &caps); + if (nt_res == HIDP_STATUS_SUCCESS) { + cur_dev->usage_page = caps.UsagePage; + cur_dev->usage = caps.Usage; + } + + HidD_FreePreparsedData(pp_data); + } + + /* Fill out the record */ + cur_dev->next = NULL; + str = device_interface_detail_data->DevicePath; + if (str) { + len = strlen(str); + cur_dev->path = (char*) calloc(len+1, sizeof(char)); + strncpy(cur_dev->path, str, len+1); + cur_dev->path[len] = '\0'; + } + else + cur_dev->path = NULL; + + /* Serial Number */ + res = HidD_GetSerialNumberString(write_handle, wstr, sizeof(wstr)); + wstr[WSTR_LEN-1] = 0x0000; + if (res) { + cur_dev->serial_number = _wcsdup(wstr); + } + + /* Manufacturer String */ + res = HidD_GetManufacturerString(write_handle, wstr, sizeof(wstr)); + wstr[WSTR_LEN-1] = 0x0000; + if (res) { + cur_dev->manufacturer_string = _wcsdup(wstr); + } + + /* Product String */ + res = HidD_GetProductString(write_handle, wstr, sizeof(wstr)); + wstr[WSTR_LEN-1] = 0x0000; + if (res) { + cur_dev->product_string = _wcsdup(wstr); + } + + /* VID/PID */ + cur_dev->vendor_id = attrib.VendorID; + cur_dev->product_id = attrib.ProductID; + + /* Release Number */ + cur_dev->release_number = attrib.VersionNumber; + + /* Interface Number. It can sometimes be parsed out of the path + on Windows if a device has multiple interfaces. See + http://msdn.microsoft.com/en-us/windows/hardware/gg487473 or + search for "Hardware IDs for HID Devices" at MSDN. If it's not + in the path, it's set to -1. */ + cur_dev->interface_number = -1; + if (cur_dev->path) { + char *interface_component = strstr(cur_dev->path, "&mi_"); + if (interface_component) { + char *hex_str = interface_component + 4; + char *endptr = NULL; + cur_dev->interface_number = strtol(hex_str, &endptr, 16); + if (endptr == hex_str) { + /* The parsing failed. Set interface_number to -1. */ + cur_dev->interface_number = -1; + } + } + } + } + +cont_close: + CloseHandle(write_handle); +cont: + /* We no longer need the detail data. It can be freed */ + free(device_interface_detail_data); + + device_index++; + + } + + /* Close the device information handle. */ + SetupDiDestroyDeviceInfoList(device_info_set); + + return root; + +} + +void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs) +{ + /* TODO: Merge this with the Linux version. This function is platform-independent. */ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + + +HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + /* TODO: Merge this functions with the Linux version. This function should be platform independent. */ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device *handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open); + } + + hid_free_enumeration(devs); + + return handle; +} + +HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path) +{ + hid_device *dev; + HIDP_CAPS caps; + PHIDP_PREPARSED_DATA pp_data = NULL; + BOOLEAN res; + NTSTATUS nt_res; + + if (hid_init() < 0) { + return NULL; + } + + dev = new_hid_device(); + + /* Open a handle to the device */ + dev->device_handle = open_device(path, FALSE); + + /* Check validity of write_handle. */ + if (dev->device_handle == INVALID_HANDLE_VALUE) { + /* Unable to open the device. */ + register_error(dev, "CreateFile"); + goto err; + } + + /* Set the Input Report buffer size to 64 reports. */ + res = HidD_SetNumInputBuffers(dev->device_handle, 64); + if (!res) { + register_error(dev, "HidD_SetNumInputBuffers"); + goto err; + } + + /* Get the Input Report length for the device. */ + res = HidD_GetPreparsedData(dev->device_handle, &pp_data); + if (!res) { + register_error(dev, "HidD_GetPreparsedData"); + goto err; + } + nt_res = HidP_GetCaps(pp_data, &caps); + if (nt_res != HIDP_STATUS_SUCCESS) { + register_error(dev, "HidP_GetCaps"); + goto err_pp_data; + } + dev->output_report_length = caps.OutputReportByteLength; + dev->input_report_length = caps.InputReportByteLength; + HidD_FreePreparsedData(pp_data); + + dev->read_buf = (char*) malloc(dev->input_report_length); + + return dev; + +err_pp_data: + HidD_FreePreparsedData(pp_data); +err: + free_hid_device(dev); + return NULL; +} + +int HID_API_EXPORT HID_API_CALL hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + DWORD bytes_written; + BOOL res; + + OVERLAPPED ol; + unsigned char *buf; + memset(&ol, 0, sizeof(ol)); + + /* Make sure the right number of bytes are passed to WriteFile. Windows + expects the number of bytes which are in the _longest_ report (plus + one for the report number) bytes even if the data is a report + which is shorter than that. Windows gives us this value in + caps.OutputReportByteLength. If a user passes in fewer bytes than this, + create a temporary buffer which is the proper size. */ + if (length >= dev->output_report_length) { + /* The user passed the right number of bytes. Use the buffer as-is. */ + buf = (unsigned char *) data; + } else { + /* Create a temporary buffer and copy the user's data + into it, padding the rest with zeros. */ + buf = (unsigned char *) malloc(dev->output_report_length); + memcpy(buf, data, length); + memset(buf + length, 0, dev->output_report_length - length); + length = dev->output_report_length; + } + + res = WriteFile(dev->device_handle, buf, length, NULL, &ol); + + if (!res) { + if (GetLastError() != ERROR_IO_PENDING) { + /* WriteFile() failed. Return error. */ + register_error(dev, "WriteFile"); + bytes_written = -1; + goto end_of_function; + } + } + + /* Wait here until the write is done. This makes + hid_write() synchronous. */ + res = GetOverlappedResult(dev->device_handle, &ol, &bytes_written, TRUE/*wait*/); + if (!res) { + /* The Write operation failed. */ + register_error(dev, "WriteFile"); + bytes_written = -1; + goto end_of_function; + } + +end_of_function: + if (buf != data) + free(buf); + + return bytes_written; +} + + +int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + DWORD bytes_read = 0; + size_t copy_len = 0; + BOOL res; + + /* Copy the handle for convenience. */ + HANDLE ev = dev->ol.hEvent; + + if (!dev->read_pending) { + /* Start an Overlapped I/O read. */ + dev->read_pending = TRUE; + memset(dev->read_buf, 0, dev->input_report_length); + ResetEvent(ev); + res = ReadFile(dev->device_handle, dev->read_buf, dev->input_report_length, &bytes_read, &dev->ol); + + if (!res) { + if (GetLastError() != ERROR_IO_PENDING) { + /* ReadFile() has failed. + Clean up and return error. */ + CancelIo(dev->device_handle); + dev->read_pending = FALSE; + goto end_of_function; + } + } + } + + if (milliseconds >= 0) { + /* See if there is any data yet. */ + res = WaitForSingleObject(ev, milliseconds); + if (res != WAIT_OBJECT_0) { + /* There was no data this time. Return zero bytes available, + but leave the Overlapped I/O running. */ + return 0; + } + } + + /* Either WaitForSingleObject() told us that ReadFile has completed, or + we are in non-blocking mode. Get the number of bytes read. The actual + data has been copied to the data[] array which was passed to ReadFile(). */ + res = GetOverlappedResult(dev->device_handle, &dev->ol, &bytes_read, TRUE/*wait*/); + + /* Set pending back to false, even if GetOverlappedResult() returned error. */ + dev->read_pending = FALSE; + + if (res && bytes_read > 0) { + if (dev->read_buf[0] == 0x0) { + /* If report numbers aren't being used, but Windows sticks a report + number (0x0) on the beginning of the report anyway. To make this + work like the other platforms, and to make it work more like the + HID spec, we'll skip over this byte. */ + bytes_read--; + copy_len = length > bytes_read ? bytes_read : length; + memcpy(data, dev->read_buf+1, copy_len); + } + else { + /* Copy the whole buffer, report number and all. */ + copy_len = length > bytes_read ? bytes_read : length; + memcpy(data, dev->read_buf, copy_len); + } + } + +end_of_function: + if (!res) { + register_error(dev, "GetOverlappedResult"); + return -1; + } + + return copy_len; +} + +int HID_API_EXPORT HID_API_CALL hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0); +} + +int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *dev, int nonblock) +{ + dev->blocking = !nonblock; + return 0; /* Success */ +} + +int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + BOOL res = HidD_SetFeature(dev->device_handle, (PVOID)data, length); + if (!res) { + register_error(dev, "HidD_SetFeature"); + return -1; + } + + return length; +} + + +int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + BOOL res; +#if 0 + res = HidD_GetFeature(dev->device_handle, data, length); + if (!res) { + register_error(dev, "HidD_GetFeature"); + return -1; + } + return 0; /* HidD_GetFeature() doesn't give us an actual length, unfortunately */ +#else + DWORD bytes_returned; + + OVERLAPPED ol; + memset(&ol, 0, sizeof(ol)); + + res = DeviceIoControl(dev->device_handle, + IOCTL_HID_GET_FEATURE, + data, length, + data, length, + &bytes_returned, &ol); + + if (!res) { + if (GetLastError() != ERROR_IO_PENDING) { + /* DeviceIoControl() failed. Return error. */ + register_error(dev, "Send Feature Report DeviceIoControl"); + return -1; + } + } + + /* Wait here until the write is done. This makes + hid_get_feature_report() synchronous. */ + res = GetOverlappedResult(dev->device_handle, &ol, &bytes_returned, TRUE/*wait*/); + if (!res) { + /* The operation failed. */ + register_error(dev, "Send Feature Report GetOverLappedResult"); + return -1; + } + + /* bytes_returned does not include the first byte which contains the + report ID. The data buffer actually contains one more byte than + bytes_returned. */ + bytes_returned++; + + return bytes_returned; +#endif +} + +void HID_API_EXPORT HID_API_CALL hid_close(hid_device *dev) +{ + if (!dev) + return; + CancelIo(dev->device_handle); + free_hid_device(dev); +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetManufacturerString(dev->device_handle, string, sizeof(wchar_t) * maxlen); + if (!res) { + register_error(dev, "HidD_GetManufacturerString"); + return -1; + } + + return 0; +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetProductString(dev->device_handle, string, sizeof(wchar_t) * maxlen); + if (!res) { + register_error(dev, "HidD_GetProductString"); + return -1; + } + + return 0; +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetSerialNumberString(dev->device_handle, string, sizeof(wchar_t) * maxlen); + if (!res) { + register_error(dev, "HidD_GetSerialNumberString"); + return -1; + } + + return 0; +} + +int HID_API_EXPORT_CALL HID_API_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + BOOL res; + + res = HidD_GetIndexedString(dev->device_handle, string_index, string, sizeof(wchar_t) * maxlen); + if (!res) { + register_error(dev, "HidD_GetIndexedString"); + return -1; + } + + return 0; +} + + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + return (wchar_t*)dev->last_error_str; +} + + +/*#define PICPGM*/ +/*#define S11*/ +#define P32 +#ifdef S11 + unsigned short VendorID = 0xa0a0; + unsigned short ProductID = 0x0001; +#endif + +#ifdef P32 + unsigned short VendorID = 0x04d8; + unsigned short ProductID = 0x3f; +#endif + + +#ifdef PICPGM + unsigned short VendorID = 0x04d8; + unsigned short ProductID = 0x0033; +#endif + + +#if 0 +int __cdecl main(int argc, char* argv[]) +{ + int res; + unsigned char buf[65]; + + UNREFERENCED_PARAMETER(argc); + UNREFERENCED_PARAMETER(argv); + + /* Set up the command buffer. */ + memset(buf,0x00,sizeof(buf)); + buf[0] = 0; + buf[1] = 0x81; + + + /* Open the device. */ + int handle = open(VendorID, ProductID, L"12345"); + if (handle < 0) + printf("unable to open device\n"); + + + /* Toggle LED (cmd 0x80) */ + buf[1] = 0x80; + res = write(handle, buf, 65); + if (res < 0) + printf("Unable to write()\n"); + + /* Request state (cmd 0x81) */ + buf[1] = 0x81; + write(handle, buf, 65); + if (res < 0) + printf("Unable to write() (2)\n"); + + /* Read requested state */ + read(handle, buf, 65); + if (res < 0) + printf("Unable to read()\n"); + + /* Print out the returned buffer. */ + for (int i = 0; i < 4; i++) + printf("buf[%d]: %d\n", i, buf[i]); + + return 0; +} +#endif + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/Classes/hidapi.h b/Classes/hidapi.h new file mode 100644 index 0000000..e58e8a4 --- /dev/null +++ b/Classes/hidapi.h @@ -0,0 +1,385 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ + +/** @file + * @defgroup API hidapi API + */ + +#ifndef HIDAPI_H__ +#define HIDAPI_H__ + +#include + +#ifdef _WIN32 + #define HID_API_EXPORT __declspec(dllexport) + #define HID_API_CALL +#else + #define HID_API_EXPORT /**< API export macro */ + #define HID_API_CALL /**< API call macro */ +#endif + +#define HID_API_EXPORT_CALL HID_API_EXPORT HID_API_CALL /**< API export and call macro*/ + +#ifdef __cplusplus +extern "C" { +#endif + struct hid_device_; + typedef struct hid_device_ hid_device; /**< opaque hidapi structure */ + + /** hidapi info structure */ + struct hid_device_info { + /** Platform-specific device path */ + char *path; + /** Device Vendor ID */ + unsigned short vendor_id; + /** Device Product ID */ + unsigned short product_id; + /** Serial Number */ + wchar_t *serial_number; + /** Device Release Number in binary-coded decimal, + also known as Device Version Number */ + unsigned short release_number; + /** Manufacturer String */ + wchar_t *manufacturer_string; + /** Product string */ + wchar_t *product_string; + /** Usage Page for this Device/Interface + (Windows/Mac only). */ + unsigned short usage_page; + /** Usage for this Device/Interface + (Windows/Mac only).*/ + unsigned short usage; + /** The USB interface which this logical device + represents. Valid on both Linux implementations + in all cases, and valid on the Windows implementation + only if the device contains more than one interface. */ + int interface_number; + + /** Pointer to the next device */ + struct hid_device_info *next; + }; + + + /** @brief Initialize the HIDAPI library. + + This function initializes the HIDAPI library. Calling it is not + strictly necessary, as it will be called automatically by + hid_enumerate() and any of the hid_open_*() functions if it is + needed. This function should be called at the beginning of + execution however, if there is a chance of HIDAPI handles + being opened by different threads simultaneously. + + @ingroup API + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_init(void); + + /** @brief Finalize the HIDAPI library. + + This function frees all of the static data associated with + HIDAPI. It should be called at the end of execution to avoid + memory leaks. + + @ingroup API + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_exit(void); + + /** @brief Enumerate the HID Devices. + + This function returns a linked list of all the HID devices + attached to the system which match vendor_id and product_id. + If @p vendor_id is set to 0 then any vendor matches. + If @p product_id is set to 0 then any product matches. + If @p vendor_id and @p product_id are both set to 0, then + all HID devices will be returned. + + @ingroup API + @param vendor_id The Vendor ID (VID) of the types of device + to open. + @param product_id The Product ID (PID) of the types of + device to open. + + @returns + This function returns a pointer to a linked list of type + struct #hid_device, containing information about the HID devices + attached to the system, or NULL in the case of failure. Free + this linked list by calling hid_free_enumeration(). + */ + struct hid_device_info HID_API_EXPORT * HID_API_CALL hid_enumerate(unsigned short vendor_id, unsigned short product_id); + + /** @brief Free an enumeration Linked List + + This function frees a linked list created by hid_enumerate(). + + @ingroup API + @param devs Pointer to a list of struct_device returned from + hid_enumerate(). + */ + void HID_API_EXPORT HID_API_CALL hid_free_enumeration(struct hid_device_info *devs); + + /** @brief Open a HID device using a Vendor ID (VID), Product ID + (PID) and optionally a serial number. + + If @p serial_number is NULL, the first device with the + specified VID and PID is opened. + + @ingroup API + @param vendor_id The Vendor ID (VID) of the device to open. + @param product_id The Product ID (PID) of the device to open. + @param serial_number The Serial Number of the device to open + (Optionally NULL). + + @returns + This function returns a pointer to a #hid_device object on + success or NULL on failure. + */ + HID_API_EXPORT hid_device * HID_API_CALL hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number); + + /** @brief Open a HID device by its path name. + + The path name be determined by calling hid_enumerate(), or a + platform-specific path name can be used (eg: /dev/hidraw0 on + Linux). + + @ingroup API + @param path The path name of the device to open + + @returns + This function returns a pointer to a #hid_device object on + success or NULL on failure. + */ + HID_API_EXPORT hid_device * HID_API_CALL hid_open_path(const char *path); + + /** @brief Write an Output report to a HID device. + + The first byte of @p data[] must contain the Report ID. For + devices which only support a single report, this must be set + to 0x0. The remaining bytes contain the report data. Since + the Report ID is mandatory, calls to hid_write() will always + contain one more byte than the report contains. For example, + if a hid report is 16 bytes long, 17 bytes must be passed to + hid_write(), the Report ID (or 0x0, for devices with a + single report), followed by the report data (16 bytes). In + this example, the length passed in would be 17. + + hid_write() will send the data on the first OUT endpoint, if + one exists. If it does not, it will send the data through + the Control Endpoint (Endpoint 0). + + @ingroup API + @param device A device handle returned from hid_open(). + @param data The data to send, including the report number as + the first byte. + @param length The length in bytes of the data to send. + + @returns + This function returns the actual number of bytes written and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_write(hid_device *device, const unsigned char *data, size_t length); + + /** @brief Read an Input report from a HID device with timeout. + + Input reports are returned + to the host through the INTERRUPT IN endpoint. The first byte will + contain the Report number if the device uses numbered reports. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data A buffer to put the read data into. + @param length The number of bytes to read. For devices with + multiple reports, make sure to read an extra byte for + the report number. + @param milliseconds timeout in milliseconds or -1 for blocking wait. + + @returns + This function returns the actual number of bytes read and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds); + + /** @brief Read an Input report from a HID device. + + Input reports are returned + to the host through the INTERRUPT IN endpoint. The first byte will + contain the Report number if the device uses numbered reports. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data A buffer to put the read data into. + @param length The number of bytes to read. For devices with + multiple reports, make sure to read an extra byte for + the report number. + + @returns + This function returns the actual number of bytes read and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_read(hid_device *device, unsigned char *data, size_t length); + + /** @brief Set the device handle to be non-blocking. + + In non-blocking mode calls to hid_read() will return + immediately with a value of 0 if there is no data to be + read. In blocking mode, hid_read() will wait (block) until + there is data to read before returning. + + Nonblocking can be turned on and off at any time. + + @ingroup API + @param device A device handle returned from hid_open(). + @param nonblock enable or not the nonblocking reads + - 1 to enable nonblocking + - 0 to disable nonblocking. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_set_nonblocking(hid_device *device, int nonblock); + + /** @brief Send a Feature report to the device. + + Feature reports are sent over the Control endpoint as a + Set_Report transfer. The first byte of @p data[] must + contain the Report ID. For devices which only support a + single report, this must be set to 0x0. The remaining bytes + contain the report data. Since the Report ID is mandatory, + calls to hid_send_feature_report() will always contain one + more byte than the report contains. For example, if a hid + report is 16 bytes long, 17 bytes must be passed to + hid_send_feature_report(): the Report ID (or 0x0, for + devices which do not use numbered reports), followed by the + report data (16 bytes). In this example, the length passed + in would be 17. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data The data to send, including the report number as + the first byte. + @param length The length in bytes of the data to send, including + the report number. + + @returns + This function returns the actual number of bytes written and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_send_feature_report(hid_device *device, const unsigned char *data, size_t length); + + /** @brief Get a feature report from a HID device. + + Make sure to set the first byte of @p data[] to the Report + ID of the report to be read. Make sure to allow space for + this extra byte in @p data[]. + + @ingroup API + @param device A device handle returned from hid_open(). + @param data A buffer to put the read data into, including + the Report ID. Set the first byte of @p data[] to the + Report ID of the report to be read. + @param length The number of bytes to read, including an + extra byte for the report ID. The buffer can be longer + than the actual report. + + @returns + This function returns the number of bytes read and + -1 on error. + */ + int HID_API_EXPORT HID_API_CALL hid_get_feature_report(hid_device *device, unsigned char *data, size_t length); + + /** @brief Close a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + */ + void HID_API_EXPORT HID_API_CALL hid_close(hid_device *device); + + /** @brief Get The Manufacturer String from a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *device, wchar_t *string, size_t maxlen); + + /** @brief Get The Product String from a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_product_string(hid_device *device, wchar_t *string, size_t maxlen); + + /** @brief Get The Serial Number String from a HID device. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *device, wchar_t *string, size_t maxlen); + + /** @brief Get a string from a HID device, based on its string index. + + @ingroup API + @param device A device handle returned from hid_open(). + @param string_index The index of the string to get. + @param string A wide string buffer to put the data into. + @param maxlen The length of the buffer in multiples of wchar_t. + + @returns + This function returns 0 on success and -1 on error. + */ + int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *device, int string_index, wchar_t *string, size_t maxlen); + + /** @brief Get a string describing the last error which occurred. + + @ingroup API + @param device A device handle returned from hid_open(). + + @returns + This function returns a string containing the last error + which occurred or NULL if none has occurred. + */ + HID_API_EXPORT const wchar_t* HID_API_CALL hid_error(hid_device *device); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/Classes/ifft.cpp b/Classes/ifft.cpp new file mode 100644 index 0000000..6a8f167 --- /dev/null +++ b/Classes/ifft.cpp @@ -0,0 +1,191 @@ +// +// File: ifft.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "ifft.h" +#include "cwt.h" +#include "cwt_emxutil.h" +#include "fft1.h" +#include "rt_nonfinite.h" +#include +#include + +// Function Definitions + +// +// Arguments : const emxArray_creal_T *x +// emxArray_creal_T *y +// Return Type : void +// +void ifft(const emxArray_creal_T *x, emxArray_creal_T *y) +{ + emxArray_creal_T *b; + int i; + int nd2; + emxArray_creal_T *b_y1; + int n2; + emxArray_real_T *costab1q; + int i1; + emxArray_real_T *costab; + emxArray_real_T *sintab; + emxArray_real_T *sintabinv; + boolean_T useRadix2; + int N2blue; + double e; + int n; + int k; + emxInit_creal_T(&b, 2); + i = b->size[0] * b->size[1]; + b->size[0] = x->size[1]; + b->size[1] = x->size[0]; + emxEnsureCapacity_creal_T(b, i); + nd2 = x->size[0]; + for (i = 0; i < nd2; i++) { + n2 = x->size[1]; + for (i1 = 0; i1 < n2; i1++) { + b->data[i1 + b->size[0] * i] = x->data[i + x->size[0] * i1]; + } + } + + emxInit_creal_T(&b_y1, 2); + emxInit_real_T(&costab1q, 2); + emxInit_real_T(&costab, 2); + emxInit_real_T(&sintab, 2); + emxInit_real_T(&sintabinv, 2); + if ((b->size[0] == 0) || (b->size[1] == 0) || (x->size[1] == 0)) { + i = b_y1->size[0] * b_y1->size[1]; + b_y1->size[0] = x->size[1]; + b_y1->size[1] = b->size[1]; + emxEnsureCapacity_creal_T(b_y1, i); + if (x->size[1] > b->size[0]) { + nd2 = b->size[1]; + for (i = 0; i < nd2; i++) { + n2 = b_y1->size[0]; + for (i1 = 0; i1 < n2; i1++) { + b_y1->data[i1 + b_y1->size[0] * i].re = 0.0; + b_y1->data[i1 + b_y1->size[0] * i].im = 0.0; + } + } + } + } else { + useRadix2 = ((x->size[1] & (x->size[1] - 1)) == 0); + get_algo_sizes(x->size[1], useRadix2, &N2blue, &nd2); + e = 6.2831853071795862 / static_cast(nd2); + n = nd2 / 2 / 2; + i = costab1q->size[0] * costab1q->size[1]; + costab1q->size[0] = 1; + costab1q->size[1] = n + 1; + emxEnsureCapacity_real_T(costab1q, i); + costab1q->data[0] = 1.0; + nd2 = n / 2 - 1; + for (k = 0; k <= nd2; k++) { + costab1q->data[k + 1] = std::cos(e * (static_cast(k) + 1.0)); + } + + i = nd2 + 2; + i1 = n - 1; + for (k = i; k <= i1; k++) { + costab1q->data[k] = std::sin(e * static_cast((n - k))); + } + + costab1q->data[n] = 0.0; + if (!useRadix2) { + n = costab1q->size[1] - 1; + n2 = (costab1q->size[1] - 1) << 1; + i = costab->size[0] * costab->size[1]; + costab->size[0] = 1; + costab->size[1] = n2 + 1; + emxEnsureCapacity_real_T(costab, i); + i = sintab->size[0] * sintab->size[1]; + sintab->size[0] = 1; + sintab->size[1] = n2 + 1; + emxEnsureCapacity_real_T(sintab, i); + costab->data[0] = 1.0; + sintab->data[0] = 0.0; + i = sintabinv->size[0] * sintabinv->size[1]; + sintabinv->size[0] = 1; + sintabinv->size[1] = n2 + 1; + emxEnsureCapacity_real_T(sintabinv, i); + for (k = 0; k < n; k++) { + sintabinv->data[k + 1] = costab1q->data[(n - k) - 1]; + } + + i = costab1q->size[1]; + for (k = i; k <= n2; k++) { + sintabinv->data[k] = costab1q->data[k - n]; + } + + for (k = 0; k < n; k++) { + costab->data[k + 1] = costab1q->data[k + 1]; + sintab->data[k + 1] = -costab1q->data[(n - k) - 1]; + } + + i = costab1q->size[1]; + for (k = i; k <= n2; k++) { + costab->data[k] = -costab1q->data[n2 - k]; + sintab->data[k] = -costab1q->data[k - n]; + } + } else { + n = costab1q->size[1] - 1; + n2 = (costab1q->size[1] - 1) << 1; + i = costab->size[0] * costab->size[1]; + costab->size[0] = 1; + costab->size[1] = n2 + 1; + emxEnsureCapacity_real_T(costab, i); + i = sintab->size[0] * sintab->size[1]; + sintab->size[0] = 1; + sintab->size[1] = n2 + 1; + emxEnsureCapacity_real_T(sintab, i); + costab->data[0] = 1.0; + sintab->data[0] = 0.0; + for (k = 0; k < n; k++) { + costab->data[k + 1] = costab1q->data[k + 1]; + sintab->data[k + 1] = costab1q->data[(n - k) - 1]; + } + + i = costab1q->size[1]; + for (k = i; k <= n2; k++) { + costab->data[k] = -costab1q->data[n2 - k]; + sintab->data[k] = costab1q->data[k - n]; + } + + sintabinv->size[0] = 1; + sintabinv->size[1] = 0; + } + + if (useRadix2) { + b_r2br_r2dit_trig(b, x->size[1], costab, sintab, b_y1); + } else { + b_dobluesteinfft(b, N2blue, x->size[1], costab, sintab, sintabinv, b_y1); + } + } + + emxFree_real_T(&sintabinv); + emxFree_real_T(&sintab); + emxFree_real_T(&costab); + emxFree_real_T(&costab1q); + emxFree_creal_T(&b); + i = y->size[0] * y->size[1]; + y->size[0] = b_y1->size[1]; + y->size[1] = b_y1->size[0]; + emxEnsureCapacity_creal_T(y, i); + nd2 = b_y1->size[0]; + for (i = 0; i < nd2; i++) { + n2 = b_y1->size[1]; + for (i1 = 0; i1 < n2; i1++) { + y->data[i1 + y->size[0] * i] = b_y1->data[i + b_y1->size[0] * i1]; + } + } + + emxFree_creal_T(&b_y1); +} + +// +// File trailer for ifft.cpp +// +// [EOF] +// diff --git a/Classes/ifft.h b/Classes/ifft.h new file mode 100644 index 0000000..5d266ba --- /dev/null +++ b/Classes/ifft.h @@ -0,0 +1,26 @@ +// +// File: ifft.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef IFFT_H +#define IFFT_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void ifft(const emxArray_creal_T *x, emxArray_creal_T *y); + +#endif + +// +// File trailer for ifft.h +// +// [EOF] +// diff --git a/Classes/log2.cpp b/Classes/log2.cpp new file mode 100644 index 0000000..3904047 --- /dev/null +++ b/Classes/log2.cpp @@ -0,0 +1,51 @@ +// +// File: log2.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "log2.h" +#include "cwt.h" +#include "rt_nonfinite.h" +#include +#include +#include + +// Function Definitions + +// +// Arguments : double x +// Return Type : double +// +double b_log2(double x) +{ + double f; + double t; + int eint; + if (x == 0.0) { + f = rtMinusInf; + } else if (x < 0.0) { + f = rtNaN; + } else if ((!rtIsInf(x)) && (!rtIsNaN(x))) { + t = frexp(x, &eint); + if (t == 0.5) { + f = static_cast(eint) - 1.0; + } else if ((eint == 1) && (t < 0.75)) { + f = std::log(2.0 * t) / 0.69314718055994529; + } else { + f = std::log(t) / 0.69314718055994529 + static_cast(eint); + } + } else { + f = x; + } + + return f; +} + +// +// File trailer for log2.cpp +// +// [EOF] +// diff --git a/Classes/log2.h b/Classes/log2.h new file mode 100644 index 0000000..1e1f2b8 --- /dev/null +++ b/Classes/log2.h @@ -0,0 +1,26 @@ +// +// File: log2.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef LOG2_H +#define LOG2_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern double b_log2(double x); + +#endif + +// +// File trailer for log2.h +// +// [EOF] +// diff --git a/Classes/main.cpp b/Classes/main.cpp index b529ba2..8a36c64 100644 --- a/Classes/main.cpp +++ b/Classes/main.cpp @@ -1,9 +1,12 @@ #include #include "Loading.h" #include "Reconstruction.h" +#include int main(int argc, char *argv[]) { + AllocConsole();//分配控制台 + freopen("CONOUT$", "w+t", stdout);//向控制台输出 QApplication a(argc, argv); Loading l; l.show(); diff --git a/Classes/rtGetInf.cpp b/Classes/rtGetInf.cpp new file mode 100644 index 0000000..0c5cc24 --- /dev/null +++ b/Classes/rtGetInf.cpp @@ -0,0 +1,54 @@ +// +// File: rtGetInf.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 + + +// +// Abstract: +// MATLAB for code generation function to initialize non-finite, Inf and MinusInf + +#include "rtGetInf.h" + +// Function: rtGetInf ================================================================== +// Abstract: +// Initialize rtInf needed by the generated code. + +real_T rtGetInf(void) +{ + return rtInf; +} + +// Function: rtGetInfF ================================================================= +// Abstract: +// Initialize rtInfF needed by the generated code. + +real32_T rtGetInfF(void) +{ + return rtInfF; +} + +// Function: rtGetMinusInf ============================================================= +// Abstract: +// Initialize rtMinusInf needed by the generated code. + +real_T rtGetMinusInf(void) +{ + return rtMinusInf; +} + +// Function: rtGetMinusInfF ============================================================ +// Abstract: +// Initialize rtMinusInfF needed by the generated code. + +real32_T rtGetMinusInfF(void) +{ + return rtMinusInfF; +} + +// +// File trailer for rtGetInf.cpp +// +// [EOF] + diff --git a/Classes/rtGetInf.h b/Classes/rtGetInf.h new file mode 100644 index 0000000..627c455 --- /dev/null +++ b/Classes/rtGetInf.h @@ -0,0 +1,33 @@ +// +// File: rtGetInf.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 + + +#ifndef RTGETINF_H +#define RTGETINF_H +#include "rtwtypes.h" +#include "rt_nonfinite.h" +#ifdef __cplusplus + +extern "C" { + +#endif + + extern real_T rtGetInf(void); + extern real32_T rtGetInfF(void); + extern real_T rtGetMinusInf(void); + extern real32_T rtGetMinusInfF(void); + +#ifdef __cplusplus + +} +#endif +#endif + +// +// File trailer for rtGetInf.h +// +// [EOF] + diff --git a/Classes/rtGetNaN.cpp b/Classes/rtGetNaN.cpp new file mode 100644 index 0000000..398a375 --- /dev/null +++ b/Classes/rtGetNaN.cpp @@ -0,0 +1,38 @@ +// +// File: rtGetNaN.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 + + +// +// Abstract: +// MATLAB for code generation function to initialize non-finite, NaN + +#include "rtGetNaN.h" + +// Function: rtGetNaN ====================================================================== +// Abstract: +// Initialize rtNaN needed by the generated code. +// NaN is initialized as non-signaling. Assumes IEEE. + +real_T rtGetNaN(void) +{ + return rtNaN; +} + +// Function: rtGetNaNF ===================================================================== +// Abstract: +// Initialize rtNaNF needed by the generated code. +// NaN is initialized as non-signaling. Assumes IEEE. + +real32_T rtGetNaNF(void) +{ + return rtNaNF; +} + +// +// File trailer for rtGetNaN.cpp +// +// [EOF] + diff --git a/Classes/rtGetNaN.h b/Classes/rtGetNaN.h new file mode 100644 index 0000000..bb042eb --- /dev/null +++ b/Classes/rtGetNaN.h @@ -0,0 +1,32 @@ +// +// File: rtGetNaN.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 + + +#ifndef RTGETNAN_H +#define RTGETNAN_H +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" +#ifdef __cplusplus + +extern "C" { + +#endif + + extern real_T rtGetNaN(void); + extern real32_T rtGetNaNF(void); + +#ifdef __cplusplus + +} +#endif +#endif + +// +// File trailer for rtGetNaN.h +// +// [EOF] + diff --git a/Classes/rt_defines.h b/Classes/rt_defines.h new file mode 100644 index 0000000..d7046b6 --- /dev/null +++ b/Classes/rt_defines.h @@ -0,0 +1,25 @@ +// +// File: rt_defines.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef RT_DEFINES_H +#define RT_DEFINES_H + +static const double RT_PI = 3.14159265358979323846; +static const float RT_PIF = 3.1415927F; +static const double RT_LN_10 = 2.30258509299404568402; +static const float RT_LN_10F = 2.3025851F; +static const double RT_LOG10E = 0.43429448190325182765; +static const float RT_LOG10EF = 0.43429449F; +static const double RT_E = 2.7182818284590452354; +static const float RT_EF = 2.7182817F; + +#endif + +// +// File trailer for rt_defines.h +// +// [EOF] +// diff --git a/Classes/rt_nonfinite.cpp b/Classes/rt_nonfinite.cpp new file mode 100644 index 0000000..0727629 --- /dev/null +++ b/Classes/rt_nonfinite.cpp @@ -0,0 +1,79 @@ +// +// File: rt_nonfinite.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 + + +// +// Abstract: +// MATLAB for code generation function to initialize non-finites, +// (Inf, NaN and -Inf). + +#include "rt_nonfinite.h" +#include +#include + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +// Function: rt_InitInfAndNaN ================================================== +// Abstract: +// Initialize the rtInf, rtMinusInf, and rtNaN needed by the +// generated code. NaN is initialized as non-signaling. Assumes IEEE. + +void rt_InitInfAndNaN() +{ + rtNaN = std::numeric_limits::quiet_NaN(); + rtNaNF = std::numeric_limits::quiet_NaN(); + rtInf = std::numeric_limits::infinity(); + rtInfF = std::numeric_limits::infinity(); + rtMinusInf = -std::numeric_limits::infinity(); + rtMinusInfF = -std::numeric_limits::infinity(); +} + +// Function: rtIsInf ================================================== +// Abstract: +// Test if value is infinite + +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +// Function: rtIsInfF ================================================= +// Abstract: +// Test if single-precision value is infinite + +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +// Function: rtIsNaN ================================================== +// Abstract: +// Test if value is not a number + +boolean_T rtIsNaN(real_T value) +{ + return ((value!=value)? 1U : 0U); +} + +// Function: rtIsNaNF ================================================= +// Abstract: +// Test if single-precision value is not a number + +boolean_T rtIsNaNF(real32_T value) +{ + return ((value!=value)? 1U : 0U); +} + +// +// File trailer for rt_nonfinite.cpp +// +// [EOF] + diff --git a/Classes/rt_nonfinite.h b/Classes/rt_nonfinite.h new file mode 100644 index 0000000..667e150 --- /dev/null +++ b/Classes/rt_nonfinite.h @@ -0,0 +1,39 @@ +// +// File: rt_nonfinite.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 + + +#ifndef RT_NONFINITE_H +#define RT_NONFINITE_H +#include "rtwtypes.h" +#ifdef __cplusplus + +extern "C" { + +#endif + + extern real_T rtInf; + extern real_T rtMinusInf; + extern real_T rtNaN; + extern real32_T rtInfF; + extern real32_T rtMinusInfF; + extern real32_T rtNaNF; + extern void rt_InitInfAndNaN(); + extern boolean_T rtIsInf(real_T value); + extern boolean_T rtIsInfF(real32_T value); + extern boolean_T rtIsNaN(real_T value); + extern boolean_T rtIsNaNF(real32_T value); + +#ifdef __cplusplus + +} +#endif +#endif + +// +// File trailer for rt_nonfinite.h +// +// [EOF] + diff --git a/Classes/rtwtypes.h b/Classes/rtwtypes.h new file mode 100644 index 0000000..9d7832d --- /dev/null +++ b/Classes/rtwtypes.h @@ -0,0 +1,43 @@ +/* + * File: rtwtypes.h + * + * MATLAB Coder version : 4.3 + * C/C++ source code generated on : 30-Mar-2020 11:59:32 + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/*=======================================================================* + * Fixed width word size data types: * + * int64_T - signed 64 bit integers * + * uint64_T - unsigned 64 bit integers * + *=======================================================================*/ +#if defined(__APPLE__) +# ifndef INT64_T +# define INT64_T long +# define FMT64 "l" +# if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG) +# define INT_TYPE_64_IS_LONG +# endif +# endif +#endif + +#if defined(__APPLE__) +# ifndef UINT64_T +# define UINT64_T unsigned long +# define FMT64 "l" +# if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG) +# define INT_TYPE_64_IS_LONG +# endif +# endif +#endif + +#include "tmwtypes.h" +#endif + +/* + * File trailer for rtwtypes.h + * + * [EOF] + */ diff --git a/Classes/structured_light.cpp b/Classes/structured_light.cpp new file mode 100644 index 0000000..d790488 --- /dev/null +++ b/Classes/structured_light.cpp @@ -0,0 +1,458 @@ +#include "structured_light.h" + +#include +#include +#include + +namespace sl +{ + const float PIXEL_UNCERTAIN = std::numeric_limits::quiet_NaN(); + const unsigned short BIT_UNCERTAIN = 0xffff; +} + +bool sl::decode_pattern(const std::vector & images, cv::Mat & pattern_image, cv::Mat & min_max_image, cv::Size const& projector_size, unsigned flags, const cv::Mat & direct_light, unsigned m) +{ + +// for(int i = 0; i < images.size(); i++) { +// std::cout<(images.size()); + + int v_bits = 1; + int h_bits = 1; + for (int i=(1< Initial images have different size: \n"; + return false; + } + if (robust && gray_image1.size()!=direct_light.size()) + { //different size + std::cout << " --> Direct Component image has different size: \n"; + return false; + } + pattern_image = cv::Mat(gray_image1.size(), CV_32FC2); + min_max_image = cv::Mat(gray_image1.size(), CV_8UC2); + } + + //sanity check + if (gray_image1.size()!=pattern_image.size()) + { //different size + std::cout << " --> Image 1 has different size, image pair " << t << " (skipped!)\n"; + continue; + } + if (gray_image2.size()!=pattern_image.size()) + { //different size + std::cout << " --> Image 2 has different size, image pair " << t << " (skipped!)\n"; + continue; + } + + //compare + for (int h=0; h(h); + const unsigned char * row2 = gray_image2.ptr(h); + const cv::Vec2b * row_light = (robust ? direct_light.ptr(h) : NULL); + cv::Vec2f * pattern_row = pattern_image.ptr(h); + cv::Vec2b * min_max_row = min_max_image.ptr(h); + + for (int w=0; wmin_max[1] || value2>min_max[1]) + { + min_max[1] = (value1>value2?value1:value2); + } + + if (!robust) + { // [simple] pattern bit assignment + if (value1>value2) + { //set bit n to 1 + pattern[channel] += (1<Lg) + { + return (value1>value2 ? 1 : 0); + } + if (value1<=Ld && value2>=Lg) + { + return 0; + } + if (value1>=Lg && value2<=Ld) + { + return 1; + } + return BIT_UNCERTAIN; +} + +void sl::convert_pattern(cv::Mat & pattern_image, cv::Size const& projector_size, const int offset[2], bool binary) +{ + if (pattern_image.rows==0) + { //no pattern image + return; + } + if (pattern_image.type()!=CV_32FC2) + { + return; + } + + if (binary) + { + std::cout << "Converting binary code to gray\n"; + } + else + { + std::cout << "Converting gray code to binary\n"; + } + + for (int h=0; h(h); + for (int w=0; w(pattern[0]); + pattern[0] = binaryToGray(p, offset[0]) + (pattern[0] - p); + } + if (!INVALID(pattern[1])) + { + int p = static_cast(pattern[1]); + pattern[1] = binaryToGray(p, offset[1]) + (pattern[1] - p); + } + } + else + { + if (!INVALID(pattern[0])) + { + int p = static_cast(pattern[0]); + int code = grayToBinary(p, offset[0]); + + if (code<0) {code = 0;} + else if (code>=projector_size.width) {code = projector_size.width - 1;} + + pattern[0] = code + (pattern[0] - p); + } + if (!INVALID(pattern[1])) + { + int p = static_cast(pattern[1]); + int code = grayToBinary(p, offset[1]); + + if (code<0) {code = 0;} + else if (code>=projector_size.height) {code = projector_size.height - 1;} + + pattern[1] = code + (pattern[1] - p); + } + } + } + } +} + +cv::Mat sl::estimate_direct_light(const std::vector & images, float b) +{ + static const unsigned COUNT = 10; // max number of images + + unsigned count = static_cast(images.size()); + if (count<1) + { //no images + return cv::Mat(); + } + + std::cout << " --- estimate_direct_light START ---\n"; + + if (count>COUNT) + { + count = COUNT; + std::cout << "WARNING: Using only " << COUNT << " of " << count << std::endl; + } + + for (unsigned i=0; i(h)(h); + } + cv::Vec2b * row_light = direct_light.ptr(h); + + for (unsigned w=0; static_cast(w)row[i][w]) Lmin = row[i][w]; + } + + int Ld = static_cast(b1*(Lmax - Lmin) + 0.5); + int Lg = static_cast(b2*(Lmin - b*Lmax) + 0.5); + row_light[w][0] = (Lg>0 ? static_cast(Ld) : Lmax); + row_light[w][1] = (Lg>0 ? static_cast(Lg) : 0); + + //std::cout << "Ld=" << (int)row_light[w][0] << " iTotal=" <<(int) row_light[w][1] << std::endl; + } + } + + std::cout << " --- estimate_direct_light END ---\n"; + + return direct_light; +} + +cv::Mat sl::get_gray_image(cv::Mat img) +{ + if (img.channels() > 1) + { + //gray scale + cv::Mat gray_image; + cvtColor(img, gray_image, cv::COLOR_BGR2GRAY); + return gray_image; + } + cv::Mat res = img.clone(); + return res; +} + +/* From Wikipedia: http://en.wikipedia.org/wiki/Gray_code + The purpose of this function is to convert an unsigned + binary number to reflected binary Gray code. +*/ +static unsigned util_binaryToGray(unsigned num) +{ + return (num>>1) ^ num; +} + +/* From Wikipedia: http://en.wikipedia.org/wiki/Gray_code + The purpose of this function is to convert a reflected binary + Gray code number to a binary number. +*/ +static unsigned util_grayToBinary(unsigned num, unsigned numBits) +{ + for (unsigned shift = 1; shift < numBits; shift <<= 1) + { + num ^= num >> shift; + } + return num; +} + +int sl::binaryToGray(int value) {return util_binaryToGray(value);} + +inline int sl::binaryToGray(int value, unsigned offset) {return util_binaryToGray(value + offset);} +inline int sl::grayToBinary(int value, unsigned offset) {return (util_grayToBinary(value, 32) - offset);} + +cv::Mat sl::colorize_pattern(const cv::Mat & pattern_image, unsigned set, float max_value) +{ + if (pattern_image.rows==0) + { //empty image + return cv::Mat(); + } + if (pattern_image.type()!=CV_32FC2) + { //invalid image type + return cv::Mat(); + } + if (set!=0 && set!=1) + { + return cv::Mat(); + } + + cv::Mat image(pattern_image.size(), CV_8UC3); + + float max_t = max_value; + float n = 4.f; + float dt = 255.f/n; + for (int h=0; h(h); + cv::Vec3b * row2 = image.ptr(h); + for (int w=0; wmax_value || INVALID(row1[w][set])) + { //invalid value: use grey + row2[w] = cv::Vec3b(128, 128, 128); + continue; + } + //display + float t = row1[w][set]*255.f/max_t; + float c1 = 0.f, c2 = 0.f, c3 = 0.f; + if (t<=1.f*dt) + { //black -> red + float c = n*(t-0.f*dt); + c1 = c; //0-255 + c2 = 0.f; //0 + c3 = 0.f; //0 + } + else if (t<=2.f*dt) + { //red -> red,green + float c = n*(t-1.f*dt); + c1 = 255.f; //255 + c2 = c; //0-255 + c3 = 0.f; //0 + } + else if (t<=3.f*dt) + { //red,green -> green + float c = n*(t-2.f*dt); + c1 = 255.f-c; //255-0 + c2 = 255.f; //255 + c3 = 0.f; //0 + } + else if (t<=4.f*dt) + { //green -> blue + float c = n*(t-3.f*dt); + c1 = 0.f; //0 + c2 = 255.f-c; //255-0 + c3 = c; //0-255 + } + row2[w] = cv::Vec3b(static_cast(c3), static_cast(c2), static_cast(c1)); + } + } + return image; +} diff --git a/Classes/structured_light.h b/Classes/structured_light.h new file mode 100644 index 0000000..4009e95 --- /dev/null +++ b/Classes/structured_light.h @@ -0,0 +1,38 @@ +#ifndef __STRUCTURED_LIGHT_HPP__ +#define __STRUCTURED_LIGHT_HPP__ + +#include + +#ifndef _MSC_VER +# ifndef _isnan +# include +# define _isnan std::isnan +# endif +#endif + +namespace sl +{ + enum DecodeFlags {SimpleDecode = 0x00, GrayPatternDecode = 0x01, RobustDecode = 0x02}; + + extern const float PIXEL_UNCERTAIN; + extern const unsigned short BIT_UNCERTAIN; + + bool decode_pattern(const std::vector & images, cv::Mat & pattern_image, cv::Mat & min_max_image, cv::Size const& projector_size, + unsigned flags = SimpleDecode, const cv::Mat & direct_light = cv::Mat(), unsigned m = 5); + unsigned short get_robust_bit(unsigned value1, unsigned value2, unsigned Ld, unsigned Lg, unsigned m); + void convert_pattern(cv::Mat & pattern_image, cv::Size const& projector_size, const int offset[2], bool binary); + cv::Mat estimate_direct_light(const std::vector & images, float b); + + cv::Mat get_gray_image(cv::Mat img); + static inline bool INVALID(float value) {return _isnan(value)>0;} + static inline bool INVALID(const cv::Vec2f & pt) {return _isnan(pt[0]) || _isnan(pt[1]);} + static inline bool INVALID(const cv::Vec3f & pt) {return _isnan(pt[0]) || _isnan(pt[1]) || _isnan(pt[2]);} + + int binaryToGray(int value); + inline int binaryToGray(int value, unsigned offset); + inline int grayToBinary(int value, unsigned offset); + + cv::Mat colorize_pattern(const cv::Mat & pattern_image, unsigned set, float max_value); +}; + +#endif //__STRUCTURED_LIGHT_HPP__ diff --git a/Classes/tmwtypes.h b/Classes/tmwtypes.h new file mode 100644 index 0000000..a9be70c --- /dev/null +++ b/Classes/tmwtypes.h @@ -0,0 +1,757 @@ +/* + * @(#)tmwtypes.h generated by: makeheader 5.1.3 Mon Jan 8 22:14:40 2007 + * + * built from: ../../src/include/copyright.h + * ../../src/include/tmwtypes.h + */ + +#if defined(_MSC_VER) || __GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3) +#pragma once +#endif + +#ifndef tmwtypes_h +#define tmwtypes_h + + +/* + * Copyright 1984-2007 The MathWorks, Inc. + */ + + + +/* Copyright 1995-2006 The MathWorks, Inc. */ + +#ifndef __TMWTYPES__ +#define __TMWTYPES__ +/* + * File : tmwtypes.h + * Abstract: + * Data types for use with MATLAB/SIMULINK and the Real-Time Workshop. + * + * When compiling stand-alone model code, data types can be overridden + * via compiler switches. + * + * Define NO_FLOATS to eliminate reference to real_T, etc. + */ + + +#include + +#ifdef __APPLE_CC__ +#include +#endif + +#define LOGICAL_IS_A_TYPE +#define SPARSE_GENERALIZATION + +#ifdef NO_FLOATS +# define double double_not_allowed +# define float float_not_allowed +#endif /*NO_FLOATS*/ + +#ifndef NO_FLOATS + +#ifndef __MWERKS__ +# ifdef __STDC__ +# include +# else +# define FLT_MANT_DIG 24 +# define DBL_MANT_DIG 53 +# endif +#endif + +#endif /*NO_FLOATS*/ + +/* + * The following data types cannot be overridden when building MEX files. + */ +#ifdef MATLAB_MEX_FILE +# undef CHARACTER_T +# undef INTEGER_T +# undef BOOLEAN_T +# undef REAL_T +# undef TIME_T +#endif + +/* + * The uchar_T, ushort_T and ulong_T types are needed for compilers which do + * not allow defines to be specified, at the command line, with spaces in them. + */ + +typedef unsigned char uchar_T; +typedef unsigned short ushort_T; +typedef unsigned long ulong_T; + + + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +/* When used with Real Time Workshop generated code, this + * header file can be used with a variety of compilers. + * + * The compiler could be for an 8 bit embedded processor that + * only had 8 bits per integer and 16 bits per long. + * In that example, a 32 bit integer size is not even available. + * This header file should be robust to that. + * + * For the case of an 8 bit processor, the preprocessor + * may be limited to 16 bit math like its target. That limitation + * would mean that 32 bit comparisons can't be done accurately. + * To increase robustness to this, comparisons are done against + * smaller values first. An inaccurate 32 bit comparison isn't + * attempted if the 16 bit comparison has already succeeded. + * + * Limitations on preprocessor math can also be stricter than + * for the target. There are known cases where a compiler + * targeting processors with 64 bit longs can't do accurate + * preprocessor comparisons on more than 32 bits. + */ + +/* Determine the number of bits for int, long, short, and char. + * If one fails to be determined, set the number of bits to -1 + */ + +#ifndef TMW_BITS_PER_INT +# if INT_MAX == 0x7FL +# define TMW_BITS_PER_INT 8 +# elif INT_MAX == 0x7FFFL +# define TMW_BITS_PER_INT 16 +# elif INT_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_INT 32 +# else +# define TMW_BITS_PER_INT -1 +# endif +#endif + +#ifndef TMW_BITS_PER_LONG +# if LONG_MAX == 0x7FL +# define TMW_BITS_PER_LONG 8 +# elif LONG_MAX == 0x7FFFL +# define TMW_BITS_PER_LONG 16 +# elif LONG_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_LONG 32 +# else +# define TMW_BITS_PER_LONG -1 +# endif +#endif + +#ifndef TMW_BITS_PER_SHRT +# if SHRT_MAX == 0x7FL +# define TMW_BITS_PER_SHRT 8 +# elif SHRT_MAX == 0x7FFFL +# define TMW_BITS_PER_SHRT 16 +# elif SHRT_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_SHRT 32 +# else +# define TMW_BITS_PER_SHRT -1 +# endif +#endif + +#ifndef TMW_BITS_PER_SCHAR +# if SCHAR_MAX == 0x7FL +# define TMW_BITS_PER_SCHAR 8 +# elif SCHAR_MAX == 0x7FFFL +# define TMW_BITS_PER_SCHAR 16 +# elif SCHAR_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_SCHAR 32 +# else +# define TMW_BITS_PER_SCHAR -1 +# endif +#endif + +#ifndef TMW_CHAR_SIGNED +# if SCHAR_MAX == CHAR_MAX +# define TMW_CHAR_SIGNED 1 +# else +# define TMW_CHAR_SIGNED 0 +# endif +#endif + +/* It is common for one or more of the integer types + * to be the same size. For example, on many embedded + * processors, both shorts and ints are 16 bits. On + * processors used for workstations, it is quite common + * for both int and long to be 32 bits. + * When there is more than one choice for typdef'ing + * a portable type like int16_T or uint32_T, in + * concept, it should not matter which choice is made. + * However, some style guides and some code checking + * tools do identify and complain about seemingly + * irrelevant differences. For example, a code + * checking tool may complain about an implicit + * conversion from int to short even though both + * are 16 bits. To reduce these types of + * complaints, it is best to make int the + * preferred choice when more than one is available. + */ + +#ifndef INT8_T +# if TMW_BITS_PER_INT == 8 +# define INT8_T int +# elif TMW_BITS_PER_LONG == 8 +# define INT8_T long +# elif TMW_BITS_PER_SCHAR == 8 +# if TMW_CHAR_SIGNED +# define INT8_T char +# else +# define INT8_T signed char +# endif +# elif TMW_BITS_PER_SHRT == 8 +# define INT8_T short +# endif +#endif +#ifdef INT8_T + typedef INT8_T int8_T; +#endif + +#ifndef UINT8_T +# if TMW_BITS_PER_INT == 8 +# define UINT8_T unsigned int +# elif TMW_BITS_PER_LONG == 8 +# define UINT8_T unsigned long +# elif TMW_BITS_PER_SCHAR == 8 +# if TMW_CHAR_SIGNED +# define UINT8_T unsigned char +# else +# define UINT8_T char +# endif +# elif TMW_BITS_PER_SHRT == 8 +# define UINT8_T unsigned short +# endif +#endif +#ifdef UINT8_T + typedef UINT8_T uint8_T; +#endif + + +#ifndef INT16_T +# if TMW_BITS_PER_INT == 16 +# define INT16_T int +# elif TMW_BITS_PER_LONG == 16 +# define INT16_T long +# elif TMW_BITS_PER_SCHAR == 16 +# if TMW_CHAR_SIGNED +# define INT16_T char +# else +# define INT16_T signed char +# endif +# elif TMW_BITS_PER_SHRT == 16 +# define INT16_T short +# endif +#endif +#ifdef INT16_T + typedef INT16_T int16_T; +#endif + + +#ifndef UINT16_T +# if TMW_BITS_PER_INT == 16 +# define UINT16_T unsigned int +# elif TMW_BITS_PER_LONG == 16 +# define UINT16_T unsigned long +# elif TMW_BITS_PER_SCHAR == 16 +# if TMW_CHAR_SIGNED +# define UINT16_T unsigned char +# else +# define UINT16_T char +# endif +# elif TMW_BITS_PER_SHRT == 16 +# define UINT16_T unsigned short +# endif +#endif +#ifdef UINT16_T + typedef UINT16_T uint16_T; +#endif + + +#ifndef INT32_T +# if TMW_BITS_PER_INT == 32 +# define INT32_T int +# elif TMW_BITS_PER_LONG == 32 +# define INT32_T long +# elif TMW_BITS_PER_SCHAR == 32 +# if TMW_CHAR_SIGNED +# define INT32_T char +# else +# define INT32_T signed char +# endif +# elif TMW_BITS_PER_SHRT == 32 +# define INT32_T short +# endif +#endif +#ifdef INT32_T + typedef INT32_T int32_T; +#endif + + +#ifndef UINT32_T +# if TMW_BITS_PER_INT == 32 +# define UINT32_T unsigned int +# elif TMW_BITS_PER_LONG == 32 +# define UINT32_T unsigned long +# elif TMW_BITS_PER_SCHAR == 32 +# if TMW_CHAR_SIGNED +# define UINT32_T unsigned char +# else +# define UINT32_T char +# endif +# elif TMW_BITS_PER_SHRT == 32 +# define UINT32_T unsigned short +# endif +#endif +#ifdef UINT32_T + typedef UINT32_T uint32_T; +#endif + +/* The following is used to emulate smaller integer types when only + * larger types are available. For example, compilers for TI C3x/C4x DSPs + * define char and short to be 32 bits, so 8 and 16 bits are not directly + * available. This target is commonly used with RTW rapid prototyping. + * Other DSPs define char to be 16 bits, so 8 bits is not directly + * available. + */ +#ifndef INT8_T +# ifdef INT16_T + typedef INT16_T int8_T; +# else +# ifdef INT32_T + typedef INT32_T int8_T; +# endif +# endif +#endif + +#ifndef UINT8_T +# ifdef UINT16_T + typedef UINT16_T uint8_T; +# else +# ifdef UINT32_T + typedef UINT32_T uint8_T; +# endif +# endif +#endif + +#ifndef INT16_T +# ifdef INT32_T + typedef INT32_T int16_T; +# endif +#endif + +#ifndef UINT16_T +# ifdef UINT32_T + typedef UINT32_T uint16_T; +# endif +#endif + + +#ifndef NO_FLOATS + +#ifndef REAL32_T +# ifndef __MWERKS__ +# if FLT_MANT_DIG >= 23 +# define REAL32_T float +# endif +# else +# define REAL32_T float +# endif +#endif +#ifdef REAL32_T + typedef REAL32_T real32_T; +#endif + + +#ifndef REAL64_T +# ifndef __MWERKS__ +# if DBL_MANT_DIG >= 52 +# define REAL64_T double +# endif +# else +# define REAL64_T double +# endif +#endif +#ifdef REAL64_T + typedef REAL64_T real64_T; +#endif + +#endif /* NO_FLOATS*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int64_T - signed 64 bit integers * + * uint64_T - unsigned 64 bit integers * + *=======================================================================*/ + + + +#ifndef INT64_T +# if defined(__alpha) || defined(__sparcv9) || defined(__ia64) || \ + defined(__ia64__) || defined(__x86_64__) || defined(__LP64__) +# define INT64_T long +# define FMT64 "l" +# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) +# define INT64_T __int64 +# define FMT64 "I64" +# elif defined(__GNUC__) || defined(__hpux) || defined(__sun) \ + || defined(TMW_ENABLE_INT64) +# define INT64_T long long +# define FMT64 "ll" +# endif +#endif + + + +#if defined(INT64_T) +# if (__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)) + __extension__ +# endif + typedef INT64_T int64_T; +#endif + + + +#ifndef UINT64_T +# if defined(__alpha) || defined(__sparcv9) || defined(__ia64) || \ + defined(__ia64__) || defined(__x86_64__) || defined(__LP64__) +# define UINT64_T unsigned long +# define FMT64 "l" +# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) +# define UINT64_T unsigned __int64 +# define FMT64 "I64" +# elif defined(__GNUC__) || defined(__hpux) || defined(__sun) \ + || defined(TMW_ENABLE_INT64) +# define UINT64_T unsigned long long +# define FMT64 "ll" +# endif +#endif + + + +#if defined(UINT64_T) +# if (__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)) + __extension__ +# endif + typedef UINT64_T uint64_T; +#endif + +/*===========================================================================* + * Format string modifiers for using size_t variables in printf statements. * + *===========================================================================*/ + +#ifndef FMT_SIZE_T +# if defined( __GNUC__ ) || defined(_STDC_C99) +# define FMT_SIZE_T "z" +# elif defined (__WATCOMC__) +# define FMT_SIZE_T "l" +# elif defined (_WIN32 ) +# define FMT_SIZE_T "I" +# else +# define FMT_SIZE_T "l" +# endif +#endif + +/*===========================================================================* + * General or logical data types where the word size is not guaranteed. * + * real_T - possible settings include real32_T or real64_T * + * time_T - possible settings include real64_T or uint32_T * + * boolean_T * + * char_T * + * int_T * + * uint_T * + * byte_T * + *===========================================================================*/ + +#ifndef NO_FLOATS + +#ifndef REAL_T +# ifdef REAL64_T +# define REAL_T real64_T +# else +# ifdef REAL32_T +# define REAL_T real32_T +# endif +# endif +#endif +#ifdef REAL_T + typedef REAL_T real_T; +#endif + +#ifndef TIME_T +# ifdef REAL_T +# define TIME_T real_T +# endif +#endif +#ifdef TIME_T + typedef TIME_T time_T; +#endif + +#endif /* NO_FLOATS */ + +#ifndef BOOLEAN_T +# if defined(UINT8_T) +# define BOOLEAN_T UINT8_T +# else +# define BOOLEAN_T unsigned int +# endif +#endif +typedef BOOLEAN_T boolean_T; + + +#ifndef CHARACTER_T +# define CHARACTER_T char +#endif +typedef CHARACTER_T char_T; + + +#ifndef INTEGER_T +# define INTEGER_T int +#endif +typedef INTEGER_T int_T; + + +#ifndef UINTEGER_T +# define UINTEGER_T unsigned +#endif +typedef UINTEGER_T uint_T; + + +#ifndef BYTE_T +# define BYTE_T unsigned char +#endif +typedef BYTE_T byte_T; + + +/*===========================================================================* + * Define Complex Structures * + *===========================================================================*/ +#ifndef NO_FLOATS + +#ifndef CREAL32_T +# ifdef REAL32_T + typedef struct { + real32_T re, im; + } creal32_T; +# define CREAL32_T creal32_T +# endif +#endif + +#ifndef CREAL64_T +# ifdef REAL64_T + typedef struct { + real64_T re, im; + } creal64_T; +# define CREAL64_T creal64_T +# endif +#endif + +#ifndef CREAL_T +# ifdef REAL_T + typedef struct { + real_T re, im; + } creal_T; +# define CREAL_T creal_T +# endif +#endif + +#endif /* NO_FLOATS */ + +#ifndef CINT8_T +# ifdef INT8_T + typedef struct { + int8_T re, im; + } cint8_T; +# define CINT8_T cint8_T +# endif +#endif + +#ifndef CUINT8_T +# ifdef UINT8_T + typedef struct { + uint8_T re, im; + } cuint8_T; +# define CUINT8_T cuint8_T +# endif +#endif + +#ifndef CINT16_T +# ifdef INT16_T + typedef struct { + int16_T re, im; + } cint16_T; +# define CINT16_T cint16_T +# endif +#endif + +#ifndef CUINT16_T +# ifdef UINT16_T + typedef struct { + uint16_T re, im; + } cuint16_T; +# define CUINT16_T cuint16_T +# endif +#endif + +#ifndef CINT32_T +# ifdef INT32_T + typedef struct { + int32_T re, im; + } cint32_T; +# define CINT32_T cint32_T +# endif +#endif + +#ifndef CUINT32_T +# ifdef UINT32_T + typedef struct { + uint32_T re, im; + } cuint32_T; +# define CUINT32_T cuint32_T +# endif +#endif + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) /* 127 */ +#define MIN_int8_T ((int8_T)(-128)) /* -128 */ +#define MAX_uint8_T ((uint8_T)(255)) /* 255 */ +#define MIN_uint8_T ((uint8_T)(0)) + +#define MAX_int16_T ((int16_T)(32767)) /* 32767 */ +#define MIN_int16_T ((int16_T)(-32768)) /* -32768 */ +#define MAX_uint16_T ((uint16_T)(65535)) /* 65535 */ +#define MIN_uint16_T ((uint16_T)(0)) + +#define MAX_int32_T ((int32_T)(2147483647)) /* 2147483647 */ +#define MIN_int32_T ((int32_T)(-2147483647-1)) /* -2147483648 */ +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) /* 4294967295 */ +#define MIN_uint32_T ((uint32_T)(0)) + +#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) +# ifdef INT64_T +# define MAX_int64_T ((int64_T)(9223372036854775807)) +# define MIN_int64_T ((int64_T)(-9223372036854775807-1)) +# endif +# ifdef UINT64_T +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFU)) +# define MIN_uint64_T ((uint64_T)(0)) +# endif +#else +# ifdef INT64_T +# define MAX_int64_T ((int64_T)(9223372036854775807LL)) +# define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +# endif +# ifdef UINT64_T +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFLLU)) +# define MIN_uint64_T ((uint64_T)(0)) +# endif +#endif + +#ifdef _MSC_VER +/* Conversion from unsigned __int64 to double is not implemented in windows + * and results in a compile error, thus the value must first be cast to + * signed __int64, and then to double. + * + * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max, + * the macro below provides a workaround for casting a uint64 value to a double + * in windows. + */ +# define uint64_to_double(u) ( ((u) > _I64_MAX) ? \ + (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \ + (double)(__int64)(u) ) + +/* The largest double value that can be cast to uint64 in windows is the + * signed int64 max, which is 2^63-1. The macro below provides + * a workaround for casting large double values to uint64 in windows. + */ +# define double_to_uint64(d) ( ((d) > 0xffffffffffffffffu) ? \ + (unsigned __int64) 0xffffffffffffffffu : \ + ((d) < 0) ? (unsigned __int64) 0 : \ + ((d) > _I64_MAX) ? \ + (unsigned __int64) ((d) - _I64_MAX) - 1 + (unsigned __int64)_I64_MAX + 1: \ + (unsigned __int64)(d) ) +#else +# define uint64_to_double(u) ((double)(u)) +# if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__) +/* double_to_uint64 defined only for MSVC and UNIX */ +# else +# define double_to_uint64(d) ( ((d) > 0xffffffffffffffffLLU) ? \ + (unsigned long long) 0xffffffffffffffffLLU : \ + ((d) < 0) ? (unsigned long long) 0 : (unsigned long long)(d) ) +# endif +#endif + +#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined) + +#ifndef _bool_T +#define _bool_T + +typedef boolean_T bool; + +#ifndef false +#define false (0) +#endif +#ifndef true +#define true (1) +#endif + +#endif /* _bool_T */ + +#endif /* !__cplusplus */ + +/* + * This software assumes that the code is being compiled on a target using a + * 2's complement representation for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable/model) + * including the null-termination character. + */ +#define TMW_NAME_LENGTH_MAX 64 + +/* + * Maximum values for indices and dimensions + */ +#include + +#ifdef MX_COMPAT_32 +typedef int mwSize; +typedef int mwIndex; +typedef int mwSignedIndex; +#else +typedef size_t mwSize; /* unsigned pointer-width integer */ +typedef size_t mwIndex; /* unsigned pointer-width integer */ +typedef ptrdiff_t mwSignedIndex; /* a signed pointer-width integer */ +#endif + +#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32) +/* Currently 2^48 based on hardware limitations */ +# define MWSIZE_MAX 281474976710655UL +# define MWINDEX_MAX 281474976710655UL +# define MWSINDEX_MAX 281474976710655L +# define MWSINDEX_MIN -281474976710655L +#else +# define MWSIZE_MAX 2147483647UL +# define MWINDEX_MAX 2147483647UL +# define MWSINDEX_MAX 2147483647L +# define MWSINDEX_MIN -2147483647L +#endif +#define MWSIZE_MIN 0UL +#define MWINDEX_MIN 0UL + +#endif /* __TMWTYPES__ */ + +#endif /* tmwtypes_h */ diff --git a/Classes/usb.cpp b/Classes/usb.cpp new file mode 100644 index 0000000..ab5b1bb --- /dev/null +++ b/Classes/usb.cpp @@ -0,0 +1,85 @@ +/* + * usb.cpp + * + * This module has the wrapper functions to access USB driver functions. + * + * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/ + * ALL RIGHTS RESERVED + * +*/ + +//#ifdef Q_OS_WIN32 +//#include +//#endif +//#include +//#include +#include "usb.h" +//#include +//#include +#include "hidapi.h" + +/*************************************************** +* GLOBAL VARIABLES +****************************************************/ +static hid_device *DeviceHandle; //Handle to write +//In/Out buffers equal to HID endpoint size + 1 +//First byte is for Windows internal use and it is always 0 +unsigned char OutputBuffer[USB_MAX_PACKET_SIZE+1]; +unsigned char InputBuffer[USB_MAX_PACKET_SIZE+1]; + +static bool USBConnected = false; //Boolean true when device is connected + +bool USB_IsConnected() +{ + return USBConnected; +} + +int USB_Init(void) +{ + return hid_init(); +} + +int USB_Exit(void) +{ + return hid_exit(); +} + +int USB_Open() +{ + // Open the device using the VID, PID, + // and optionally the Serial number. + DeviceHandle = hid_open(MY_VID, MY_PID, NULL); + + if(DeviceHandle == NULL) + { + USBConnected = false; + return -1; + } + USBConnected = true; + return 0; +} + +int USB_Write() +{ + if(DeviceHandle == NULL) + return -1; + + return hid_write(DeviceHandle, OutputBuffer, USB_MIN_PACKET_SIZE+1); + +} + +int USB_Read() +{ + if(DeviceHandle == NULL) + return -1; + + return hid_read_timeout(DeviceHandle, InputBuffer, USB_MIN_PACKET_SIZE+1, 2000); +} + +int USB_Close() +{ + hid_close(DeviceHandle); + USBConnected = false; + + return 0; +} diff --git a/Classes/usb.h b/Classes/usb.h new file mode 100644 index 0000000..5b464b7 --- /dev/null +++ b/Classes/usb.h @@ -0,0 +1,28 @@ +/* + * usb.h + * + * This module has the wrapper functions to access USB driver functions. + * + * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/ + * ALL RIGHTS RESERVED + * +*/ + +#ifndef USB_H +#define USB_H + +#define USB_MIN_PACKET_SIZE 64 +#define USB_MAX_PACKET_SIZE 64 + +#define MY_VID 0x0451 +#define MY_PID 0x6401 + +int USB_Open(void); +bool USB_IsConnected(); +int USB_Write(); +int USB_Read(); +int USB_Close(); +int USB_Init(); +int USB_Exit(); + +#endif //USB_H diff --git a/Classes/wavCFandSD.cpp b/Classes/wavCFandSD.cpp new file mode 100644 index 0000000..492562f --- /dev/null +++ b/Classes/wavCFandSD.cpp @@ -0,0 +1,142 @@ +// +// File: wavCFandSD.cpp +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// + +// Include Files +#include "wavCFandSD.h" +#include "cwt.h" +#include "cwt_data.h" +#include "gammaln.h" +#include "rt_nonfinite.h" +#include +#include + +// Function Definitions + +// +// Arguments : const char wname[5] +// double varargin_1 +// double varargin_2 +// double *FourierFactor +// double *sigmaT +// double *cf +// Return Type : void +// +void wavCFandSD(const char wname[5], double varargin_1, double varargin_2, + double *FourierFactor, double *sigmaT, double *cf) +{ + char switch_expression; + boolean_T b_bool; + int b_index; + double cf_tmp; + double b_cf_tmp; + double d; + double d1; + double be_tmp; + double be; + double b_be_tmp; + double c_be_tmp; + double b_be; + double d_be_tmp; + double e_be_tmp; + double c_be; + double d2; + double d3; + double d4; + double d5; + double sigmaT_tmp; + double b_sigmaT_tmp; + *cf = 0.0; + *sigmaT = 0.0; + switch_expression = cv1[static_cast(wname[0]) & 127]; + b_bool = !(switch_expression != 'm'); + if (b_bool) { + b_index = 0; + } else { + b_bool = !(switch_expression != 'a'); + if (b_bool) { + b_index = 1; + } else { + b_bool = !(switch_expression != 'b'); + if (b_bool) { + b_index = 2; + } else { + b_index = -1; + } + } + } + + switch (b_index) { + case 0: + cf_tmp = std::log(varargin_1); + b_cf_tmp = std::log(varargin_2); + *cf = std::exp(1.0 / varargin_1 * (b_cf_tmp - cf_tmp)); + d = 2.0 * varargin_2 + 1.0; + d1 = (d + 2.0) / varargin_1; + gammaln(&d1); + d /= varargin_1; + d1 = d; + gammaln(&d1); + d1 = (2.0 * varargin_2 + 2.0) / varargin_1; + gammaln(&d1); + d1 = d; + gammaln(&d1); + be_tmp = 2.0 * (varargin_2 - 1.0); + be = 2.0 * varargin_2; + b_be_tmp = (varargin_2 - 1.0) + varargin_1; + c_be_tmp = 2.0 * b_be_tmp; + b_be = 2.0 * varargin_2; + d_be_tmp = (varargin_2 - 1.0) + varargin_1 / 2.0; + e_be_tmp = 2.0 * d_be_tmp; + c_be = 2.0 * varargin_2; + d1 = (be_tmp + 1.0) / varargin_1; + gammaln(&d1); + d2 = d; + gammaln(&d2); + d3 = (c_be_tmp + 1.0) / varargin_1; + gammaln(&d3); + d4 = d; + gammaln(&d4); + d5 = (e_be_tmp + 1.0) / varargin_1; + gammaln(&d5); + gammaln(&d); + sigmaT_tmp = varargin_2 / varargin_1; + b_sigmaT_tmp = 2.0 * (sigmaT_tmp * ((cf_tmp + 1.0) - b_cf_tmp)); + sigmaT_tmp = 2.0 / varargin_1 * std::log(sigmaT_tmp); + *sigmaT = std::sqrt((std::exp(((((((b_sigmaT_tmp - 2.0 * ((varargin_2 - 1.0) + / varargin_1 * ((cf_tmp + 1.0) - std::log(varargin_2 - 1.0)))) + be_tmp / + varargin_1 * ((cf_tmp + 1.0) - std::log(be_tmp))) - be / varargin_1 * + ((cf_tmp + 1.0) - std::log(be))) + sigmaT_tmp) + 2.0 * b_cf_tmp) + d1) - + d2) + std::exp(((((((b_sigmaT_tmp - 2.0 * (b_be_tmp / varargin_1 * + ((cf_tmp + 1.0) - std::log(b_be_tmp)))) + c_be_tmp / varargin_1 * ((cf_tmp + + 1.0) - std::log(c_be_tmp))) - b_be / varargin_1 * ((cf_tmp + 1.0) - std:: + log(b_be))) + sigmaT_tmp) + 2.0 * cf_tmp) + d3) - d4)) - std::exp + (((((((((b_sigmaT_tmp - 2.0 * (d_be_tmp / varargin_1 * + ((cf_tmp + 1.0) - std::log(d_be_tmp)))) + e_be_tmp / varargin_1 * ((cf_tmp + + 1.0) - std::log(e_be_tmp))) - c_be / varargin_1 * ((cf_tmp + 1.0) - std:: + log(c_be))) + sigmaT_tmp) + 0.69314718055994529) + b_cf_tmp) + cf_tmp) + + d5) - d)); + break; + + case 1: + *cf = 6.0; + *sigmaT = 1.4142135623730951; + break; + + case 2: + *cf = 5.0; + *sigmaT = 5.847705; + break; + } + + *FourierFactor = 6.2831853071795862 / *cf; +} + +// +// File trailer for wavCFandSD.cpp +// +// [EOF] +// diff --git a/Classes/wavCFandSD.h b/Classes/wavCFandSD.h new file mode 100644 index 0000000..89b72be --- /dev/null +++ b/Classes/wavCFandSD.h @@ -0,0 +1,27 @@ +// +// File: wavCFandSD.h +// +// MATLAB Coder version : 4.3 +// C/C++ source code generated on : 30-Mar-2020 11:59:32 +// +#ifndef WAVCFANDSD_H +#define WAVCFANDSD_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "omp.h" +#include "cwt_types.h" + +// Function Declarations +extern void wavCFandSD(const char wname[5], double varargin_1, double varargin_2, + double *FourierFactor, double *sigmaT, double *cf); + +#endif + +// +// File trailer for wavCFandSD.h +// +// [EOF] +// diff --git a/UI/Reconstruction.ui b/UI/Reconstruction.ui index 7f28cda..7c95937 100644 --- a/UI/Reconstruction.ui +++ b/UI/Reconstruction.ui @@ -69,7 +69,7 @@ - 2 + 0 @@ -603,7 +603,7 @@ Qt::AlignCenter - + 10 @@ -612,11 +612,6 @@ 181 - - - 9 - - @@ -1265,6 +1260,15 @@ + + + + 0 + 0 + 0 + + + @@ -1402,6 +1406,15 @@ + + + + 0 + 0 + 0 + + + @@ -1539,6 +1552,15 @@ + + + + 0 + 0 + 0 + + + @@ -1746,5 +1768,22 @@ - + + + spinBox + valueChanged(int) + spinBox + setValue(int) + + + 1146 + 98 + + + 1146 + 98 + + + +