想了一下使用乘子法还是可行的/做一个简化.在约束C(xn) 在C（xn-1）处线性展开 （n是时间步骤）
具体推导留作备份等有时间了去代码实现

3式是一个典型的LCP问题 用PGS就行 左边的系数部分依然是常量 当然整个global solve步骤增加了些运算量。