public BehaviourController(Character b, IKVMCController llc) { this.bip = b; this.lowLCon = llc; upperBodyPos = Vector3.zero; sagittalStepWidth = 0; coronalStepWidth = 0; stepHeight = 0; swingStepLength = 0; standStepLength = 0; maxStepHeight = llc.getLegLength() * 1.0f; stepLayerMask = ~((1 << (int)ColliderLayer.roleLayer) | (1 << (int)ColliderLayer.ignoreAllLayer) | (1 << (int)ColliderLayer.waterLayer) | (1 << (int)ColliderLayer.waterRipple) | (1 << (int)ColliderLayer.ignoreRaycastLayer) | (1 << (int)ColliderLayer.propsLayer) | (1 << (int)ColliderLayer.touchPointLayer)); stepRaycastHits = new ArrayList(); }
virtual public void adjustStepHeight() { Vector3 footPos = lowLCon.getSwingFoot().getWorldBottomPosition(); //Vector3 swingFootV = lowLCon.getSwingFoot().getAbsoluteVelocityForLocalPoint(lowLCon.getSwingFoot().getLocalBottomPosition()); Vector3 swingFootV = lowLCon.getCharacterFrame() * (lowLCon.getVelSagittal() * lowLCon.getCharacter().getLocalFrontAxis() - lowLCon.getVelDCoronal() * lowLCon.getCharacter().getLocalLeftAxis()); swingFootV[lowLCon.getWorldUpAxisID()] = 0; //swingFootV.Normalize(); Vector3 rayPos = footPos + 0.7f * swingFootV; float rayStartY = lowLCon.getComPosition() [lowLCon.getWorldUpAxisID()]; Vector3 rayStartPos = rayPos; rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY; float rayEndY = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()]; Vector3 rayEndPos = rayPos; rayEndPos[lowLCon.getWorldUpAxisID()] = rayEndY; RaycastHit raycastHit = new RaycastHit(); if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask)) { Debug.DrawLine(rayStartPos, rayEndPos, Color.blue); lowLCon.swingFootFrontPosition = raycastHit.point; } else { Vector3 pos = lowLCon.getSwingHip().getChildJointWorldPosition() - lowLCon.getLegLength() * SimGlobals.upAxis; Debug.DrawLine(pos, pos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis, Color.blue); lowLCon.swingFootFrontPosition = pos - lowLCon.additionHeight * SimGlobals.upAxis; } //Debug.Log("aaaaaaaaaaaaa "+lowLCon.swingFootFrontPosition); rayPos = footPos; rayStartPos = rayPos; rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY; rayEndPos = rayPos; rayEndPos [lowLCon.getWorldUpAxisID()] = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()]; if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask)) { Debug.DrawLine(rayStartPos, rayEndPos, Color.green); lowLCon.swingFootBasePosition = raycastHit.point; lowLCon.swingFootBaseNormal = raycastHit.normal; } else { lowLCon.swingFootBasePosition = lowLCon.swingFootFrontPosition; lowLCon.swingFootBaseNormal = Vector3.zero; } //Debug.Log("bbbbbbbbbbbbb "+lowLCon.swingFootBasePosition); // Debug.Log("aaaaaaaaaaaaaaaaa "+lowLCon.swingFootBaseNormal); footPos = lowLCon.getStanceFoot().getWorldBottomPosition(); rayPos = footPos; rayEndY = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()]; rayStartPos = rayPos; rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY; rayEndPos = rayPos; rayEndPos[lowLCon.getWorldUpAxisID()] = rayEndY; if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask)) { Debug.DrawLine(rayStartPos, rayEndPos, Color.green); lowLCon.standFootBasePosition = raycastHit.point; lowLCon.standFootBaseNormal = raycastHit.normal; } else { Vector3 pos = lowLCon.getStandHip().getChildJointWorldPosition() - lowLCon.getLegLength() * SimGlobals.upAxis; Debug.DrawLine(pos, pos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis, Color.green); lowLCon.standFootBasePosition = pos - lowLCon.additionHeight * SimGlobals.upAxis; lowLCon.standFootBaseNormal = Vector3.zero; } // Debug.Log("bbbbbbbbbbbbbbbbb "+lowLCon.standFootBaseNormal); lowLCon.swingToStandBaseVector = Quaternion.Inverse(lowLCon.getCharacterFrame()) * (lowLCon.standFootBasePosition - lowLCon.swingFootBasePosition); //Debug.Log("bbbbbbbbbbbbb "+lowLCon.swingToStandBaseVector); float h = lowLCon.getDoubleStanceCOMError().magnitude * 1.6f; h = Mathf.Clamp(h, lowLCon.getStepHeight(), maxStepHeight); lowLCon.swingLegPanicHeight = h - lowLCon.getStepHeight(); //Debug.Log("bbbbbbbbbbbbbbbbb "+lowLCon.swingLegPanicHeight); }