static HDCallbackCode GravityWellCallback(IntPtr pUserData) { const double kStiffness = 0.175; //(N/mm) const double kGravityWellInfluence = 50; //Box Size:50x50x50(mm) Vector3D position = new Vector3D(); Vector3D force = new Vector3D(); Vector3D positionTwell = new Vector3D(); uint hHD = HDAPI.hdGetCurrentDevice(); //触觉技术框架开始。(一般来说,所有与状态相关的触觉调用都应该在一个框架内进行。) HDAPI.hdBeginFrame(hHD); //获取设备的当前位置 HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, ref position); //Console.WriteLine("Vector3D:{0} {1} {2}", position.X, position.Y, position.Z); force.ResetZero(); // positionTwell = wellPos - position //创建一个从设备位置到重力井中心的矢量 Vector3D.Subtrace(ref positionTwell, ref wellPos, ref position); //如果装置位置在重力井中心一定距离内,则向重力井中心施加弹簧力。 //力的计算不同于传统的重力体,因为装置离中心越近,井所施加的力就越小; //该装置的行为就像弹簧连接在它自己和井的中心。 if (InForce && Vector3D.Magnitude(ref positionTwell) < kGravityWellInfluence) { // F = k * x //F:力的单位为牛顿(N) //k: 井的刚度(N / mm) //x: 从设备端点位置到井中心的向量。 Vector3D.Scale(ref force, ref positionTwell, kStiffness); } if (!InForce && Vector3D.Magnitude(ref positionTwell) >= kGravityWellInfluence) { Vector3D.Scale(ref force, ref positionTwell, kStiffness); } //把力传送到设备上 HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, ref force); //End haptics frame. HDAPI.hdEndFrame(hHD); //check error HDErrorInfo error = HDAPI.hdGetError(); if (error.CheckedError()) { Console.WriteLine("渲染重力时检测到错误"); if (error.IsSchedulerError()) { Console.WriteLine("调度器错误。"); return(HDCallbackCode.HD_CALLBACK_DONE); } } return(HDCallbackCode.HD_CALLBACK_CONTINUE); }
static HDCallbackCode GetSyncPos(IntPtr pUserData) { Vector3D v3 = HDDLLUtils.IntPtrToStruct <Vector3D>(pUserData); HDAPI.hdBeginFrame(v3.HHD); //将获取的坐标位置放入结构数据前三个double字节中 HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, pUserData); HDAPI.hdEndFrame(v3.HHD); return(HDCallbackCode.HD_CALLBACK_DONE); }
static HDCallbackCode AnchoredSpringForceHandler(IntPtr pUserData) { uint hHD = (uint)pUserData; double[] force = new double[3] { 0.0, 0.0, 0.0 }; HDAPI.hdBeginFrame(hHD); double[] position = new double[3]; HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, position); int currButtons = 0; int lastButtons = 0; HDAPI.hdGetIntegerv(HDGetParameters.HD_LAST_BUTTONS, ref lastButtons); HDAPI.hdGetIntegerv(HDGetParameters.HD_CURRENT_BUTTONS, ref currButtons); //按下 Button 1 if ((currButtons & (int)HDButtonMasks.HD_DEVICE_BUTTON_1) != 0 && (lastButtons & (int)HDButtonMasks.HD_DEVICE_BUTTON_1) == 0) { anchor = position; renderForce = true; Console.WriteLine("gSpringStiffness:{0}", gSpringStiffness); //Console.WriteLine("Button Down:{0} {1} {2}", position[0], position[1], position[2]); } else if ((currButtons & (int)HDButtonMasks.HD_DEVICE_BUTTON_1) == 0 && (lastButtons & (int)HDButtonMasks.HD_DEVICE_BUTTON_1) != 0) { renderForce = false; //向设备发送零力,否则它将继续呈现最后发送的力 HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, force); } if (renderForce) { //计算弹簧力为 F = k * (anchor - position),这将吸引设备位置朝向锚点位置 Vector3D.Subtrace(ref force, anchor, position); Vector3D.ScaleInPlace(ref force, gSpringStiffness); HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, force); } HDAPI.hdEndFrame(hHD); return(HDCallbackCode.HD_CALLBACK_CONTINUE); }
static HDCallbackCode FrictionlessSphereCallback(IntPtr pUserData) { uint hHD = HDAPI.hdGetCurrentDevice(); HDAPI.hdBeginFrame(hHD); //获取坐标位置 Vector3D position = new Vector3D(); HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, ref position); //Console.WriteLine("{0} {1} {2}", position.X, position.Y, position.Z); //计算设备和球体中心之间的距离。 Vector3D res = position - spherePosition; double distance = Vector3D.Magnitude(ref res); //如果用户在球体内,即用户到球体中心的距离小于球体半径, //则用户正在穿透球体,应命令一个力将用户推向表面。 if (distance < radius) { //计算穿透距离。 double penetrationDistance = radius - distance; //在力的方向上创建一个单位矢量,它总是从球体的中心通过用户的位置向外。 Vector3D forceDirection = (position - spherePosition) / distance; //使用 F = k * x 创建一个远离中心的力向量 //球体与穿透距离成正比,并受物体刚度的制约。 double k = stiffness; Vector3D x = penetrationDistance * forceDirection; Vector3D f = k * x; HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, ref f); } HDAPI.hdEndFrame(hHD); HDErrorInfo error = HDAPI.hdGetError(); if (error.CheckedError()) { Console.WriteLine("主调度程序回调期间出错."); if (error.IsSchedulerError()) { return(HDCallbackCode.HD_CALLBACK_DONE); } } return(HDCallbackCode.HD_CALLBACK_CONTINUE); }
static void Main(string[] args) { hHD = HDAPI.hdInitDevice(null); error = HDAPI.hdGetError(); if (error.CheckedError()) { Console.WriteLine("Init Error."); Console.ReadKey(); return; } //获取支持的校准样式,因为一些设备可能支持多种类型的校准 HDAPI.hdGetIntegerv(HDGetParameters.HD_CALIBRATION_STYLE, ref supportedCalibrationStyles); Console.WriteLine("supportedCalibrationStyles:{0} {1}", supportedCalibrationStyles, Enum.GetName(typeof(HDCalibrationStyles), supportedCalibrationStyles)); //使用墨水池校准 HDAPI.hdUpdateCalibration(HDCalibrationStyles.HD_CALIBRATION_INKWELL); //启动 Servo Loop HDAPI.hdStartScheduler(); #if 直接获取坐标位置输出 double[] pPosition = new double[3]; int buttons = 0; while (true) { if (HDAPI.hdCheckCalibration() == HDCalibrationCodes.HD_CALIBRATION_NEEDS_UPDATE) { HDAPI.hdUpdateCalibration(HDCalibrationStyles.HD_CALIBRATION_INKWELL); } HDAPI.hdBeginFrame(hHD); HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, pPosition); HDAPI.hdGetIntegerv(HDGetParameters.HD_CURRENT_BUTTONS, ref buttons); HDAPI.hdEndFrame(hHD); Console.WriteLine("Button Status:Btn1:{0} Btn2:{1} Btn3:{2} Btn4:{3}", buttons & (int)HDButtonMasks.HD_DEVICE_BUTTON_1, buttons & (int)HDButtonMasks.HD_DEVICE_BUTTON_2, buttons & (int)HDButtonMasks.HD_DEVICE_BUTTON_3, buttons & (int)HDButtonMasks.HD_DEVICE_BUTTON_4); Console.WriteLine("Position: X:{0} Y:{1} Z:{2}", pPosition[0], pPosition[1], pPosition[2]); Thread.Sleep(100); } #endif #if 使用调度器同步输出位置 PositionSynchronous = GetSyncPos; Vector3D v3 = new Vector3D(); v3.HHD = hHD; IntPtr pPosition = OHUtils.StructToIntPtr(v3); while (true) { HDAPI.hdScheduleSynchronous(PositionSynchronous, pPosition, HDSchedulerPriority.HD_DEFAULT_SCHEDULER_PRIORITY); v3 = OHUtils.IntPtrToStruct <Vector3D>(pPosition); Console.WriteLine("Position: X:{0} Y:{1} Z:{2}", v3.X, v3.Y, v3.Z); //Sleep一下,不然打印的数据太多,看不清,实际项目是不需要的 //Thread.Sleep(100); } #endif //停止 Servo Loop HDAPI.hdStopScheduler(); //禁用设备 HDAPI.hdDisableDevice(hHD); Console.ReadKey(); return; }